/*-------------------------------------------------------
 * 低速ブースト機能
 *   実装の概要
 *   - TMR1割込発生ごとに通常期間とブースト期間を交代させる
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "adc.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "pp-sb.h"

/* 低速ブーストをOnとする速度の最大値 */
#define SB_SPEED_MAX      512

/* 通常期間/ブースト期間 */
enum sb_state_e {
  SB_STATE_NORMAL,
  SB_STATE_SBOOST,
};
volatile static enum sb_state_e sb_state = 0;

/* モーター出力のPWMデューティ値 */
volatile uint16_t sb_mduty = 0;  

/* タイマ設定値 */
volatile static uint8_t  sb_t_fh = 0; /* TMR1(H): 周波数 */
volatile static uint8_t  sb_t_fl = 0; /* TMR1(L): 周波数 */
volatile static uint8_t  sb_t_ph = 0; /* TMR1(H): パルス幅 */
volatile static uint8_t  sb_t_pl = 0; /* TMR1(L): パルス幅 */

void TMR1_Init(void);

/*-------------------------------------------------------
 * 初期化
 *-------------------------------------------------------*/
void pp_sb_init(void)
{
  sb_state = SB_STATE_NORMAL;
  sb_mduty = 0; 
  sb_t_fh = 0;
  sb_t_fl = 0;
  sb_t_ph = 0;
  sb_t_pl = 0;
  pp_param_read();
  pp_sb_duty_set(0);
  TMR1_Init();
}

/*-------------------------------------------------------
 * モーターPWMデューティ比設定 
 *-------------------------------------------------------*/
void pp_sb_duty_set(uint16_t mduty)
{
  /* 割り込み禁止/許可状態を保存 */ 
  int8_t ogie  = GIE;
  int8_t opeie = PEIE;
  /* 割り込み禁止 */ 
  /*   sb_t_phとsb_t_pl及びsb_t_fhとsb_t_flそれぞれを
   *   アトミックに更新するため */
  GIE  = 0;
  PEIE = 0;
  /* モーターPWMデューティ値(通常出力値) */
  sb_mduty = mduty;
  /* 低速ブーストパルス幅の計算:
   * - TMR1カウント値 pw=0x0000 のとき
   *     TMR1割込間隔: 0.00 [mSec]
   *     = Prescale * 0x0000 / (FOCS/4) [Sec] 
   *     = 8 * 0x0000 / (32*10^6 / 4) [Sec] 
   *     = 0.00 * 10^{-3} [Sec] 
   * - TMR1カウント値 pw=0x2000 のとき
   *     TMR1割込間隔: 8.192 [mSec]
   *     = Prescale * 0x2000 / (FOCS/4) [Sec] 
   *     = 8 * 0x2000 / (32*10^6 / 4) [Sec] 
   *     = 8.192 * 10^{-3} [Sec] 
   */
  /* p_sbpwdtの値[0..0x0400) を pw:[0..0x2000) へスケール */
  uint16_t pw = p_sbpwdt << 3; 
  if (pw < 1) {
    pw = 1;
  }
  uint16_t sb_t_p = 0x0000 - pw;
  sb_t_ph = (uint8_t)(sb_t_p >> 8);
  sb_t_pl = (uint8_t)(sb_t_p & 0xFF);
  /* 低速ブースト周波数の計算:
   * - TMR1カウント数 cy=0x1000 のとき
   *     TMR1割込間隔: 4.096 [mSec]
   *     = Prescale * 0x1000 / (FOCS/4) [Sec] 
   *     = 8 * 0x1000 / (32*10^6 / 4) [Sec] 
   *     = 4.096 * 10^{-3} [Sec] 
   * - TMR1カウント値 cy=0x8000 のとき
   *     TMR1割込間隔: 32.768 [mSec]
   *     = Prescale * 0x8000 / (FOCS/4) [Sec] 
   *     = 8 * 0x8000 / (32*10^6 / 4) [Sec] 
   *     = 32.768 * 10^{-3} [Sec] 
   */
  /* p_sbfreqの値[0..0x0400) を cy:[0x4000..0x8000) へ変換 */
  uint16_t cy = (SPEED_MAX - p_sbfreq) << 4; /* [0x0000..0x4000) */
  cy = cy + 0x4000;                          /* [0x4000..0x8000) */
  uint16_t sb_t_f = 0x0000 - cy; 
  sb_t_fh = (uint8_t)(sb_t_f >> 8);
  sb_t_fl = (uint8_t)(sb_t_f & 0xFF);
  /* 割り込み禁止/許可状態を復元 */ 
  GIE  = ogie;
  PEIE = opeie;
}

/*-------------------------------------------------------
 * TMR1割込: 通常期間とブースト期間の交替
 *-------------------------------------------------------*/
void TMR1_isr(void)
{
  /* TMR1の動作を停止 */
  T1CONbits.TMR1ON = 0;
  /* モーターPWMデューティサイクル値を求める */
  uint16_t md = 0;
  if ((sb_t_ph == 0) && (sb_t_pl == 0)) { /* パルス幅 0 */
    /* 常にブーストしない */
    md = sb_mduty;
  } else {
    /* ブースト期間/通常期間の交替を繰り返す */
    switch (sb_state) {
    default:
    case SB_STATE_NORMAL:         /* 通常期間の終了 */
      sb_state = SB_STATE_SBOOST; /* ブースト期間の開始 */
      TMR1H = sb_t_ph;
      TMR1L = sb_t_pl;
      if (sb_mduty == 0) {               /* 停車中 */
        md = 0;
      } else if (SW_SBEN() &&
                 (sb_mduty < SB_SPEED_MAX)) { /* ブーストON */
        md = SB_SPEED_MAX;
      } else {                                /* ブーストOFF */
        md = sb_mduty;
      }
      break;
    case SB_STATE_SBOOST:         /* ブースト期間の終了 */
      sb_state = SB_STATE_NORMAL; /* 通常期間の開始 */
      TMR1H = sb_t_fh;
      TMR1L = sb_t_fl;
      md = sb_mduty;
      break;
    }
  }
  /* 常点灯処理 */
  if (md < p_light) {
    md = p_light;
  }
  /* モーターPWMデューティサイクル値を出力 */
  MOTOR_PWM_OUT(md);
  /* TMR1の動作を再開 */
  T1CONbits.TMR1ON = 1;
}

/*--------------------------------------------*/
/* TMR1 */
void TMR1_Init(void)
{
  T1CONbits.TMR1ON = 0; /* Disabled */ 
  T1CONbits.CKPS   = 3; /* Prescale: 1:8  */
  T1CLKbits.CS     = 1; /* FOSC/4 */
  T1GCON = 0x00;
  TMR1H = 0;
  TMR1L = 0;
  TMR1IF  = 0;
  TMR1IE  = 1;
  T1CONbits.TMR1ON = 1; /* Enabled */ 
}
