/*-------------------------------------------------------
 * パワーパック走行制御 (赤外線式テレビリモコン)
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "adc.h"
#include "io.h"
#include "ir-frame.h"
#include "keysym.h"
#include "rc.h"
#include "cmd.h"
#include "led-ir-mon.h"
#include "if.h"
#include "if-toq.h"
#include "pp.h"
#include "pp-const.h"
#include "pp-sb.h"
#include "incr-lin.h"

uint16_t   speed = 0;
uint16_t   target_speed = 0;    /* リモコンからの制御 */
enum dir_e dir_next = DIR_NONE; /* リモコンからの制御 */
enum dir_e dir_curr = DIR_NONE;
uint8_t    pp_reset = 0;        /* リモコンからのセット要求*/

/*-------------------------------------------------------
 * 走行制御 (リモコン運転モード/手動運転モード)
 *-------------------------------------------------------*/
void pp_control(void)
{
  /* モータドライバIC のリセット */
  MotorDriver_TB6643KQ_Reset();
  /* 現在走行方向初期化 */
  dir_next = DIR_NONE;
  dir_curr = DIR_NONE;
  pp_motor_dir_set(dir_curr);
  /* 現在走行速度初期化 */
  speed = 0;
  /* 目標走行速度初期化 */
  target_speed = 0;
  /* 速度制御初期化 */
  incr_lin_init(0);
  pp_sb_duty_set(0);
  /* インジケータLEDの点滅時間管理 */
  uint16_t t_led = 0;
  /* 状態インジケータLED */
  LED_INDICATOR_OFF();
  /* 停車中表示LED */
  LED_STOPPED_OFF();
  /* コマンド実行の初期化 (リモコン制御モード時) */
  if (fsm_ctrmode == CTRMODE_REMOTE) {
    cmd_init();
  }
  /* 主ループ */
  for (;;) {
    __delay_ms(RUN_INTERVAL);
    CLRWDT();
    /* 緊急停止 */
    if (SW_EMG_STOP()) { 
      break;
    }
    /* ポイント切替制御 */
    if_toq_next(RUN_INTERVAL);
    /* パラメータの読込 */
    pp_param_read();
    /* 走行制御 */
    if (fsm_ctrmode == CTRMODE_REMOTE) { /* リモコン制御モード */
      led_ir_mon_interval(RUN_INTERVAL);
      if (ir_rcv_flag != 0) {
        if (cmd_action(rc_keysym(ir_data_buff))) {
          led_ir_mon_on();
        }
        ir_rcv_flag = 0;
        if (pp_reset != 0) {
          break;
        }
      }
    } else {                             /* 手動制御モード */
      /* 走行方向の読込 */
      dir_next = pp_dir_read();
      /* 目標走行速度 */
      target_speed = pp_controler_speed(speed);
    }
    /* 走行方向の切替(必要に応じて) */
    if (dir_curr == dir_next) { /* 同一の走行方向が継続 */
      if (dir_next == DIR_NONE) {
        LED_INDICATOR_OFF(); /* ニュートラル */
      } else {
        LED_INDICATOR_ON();  /* 前進/後進*/
      }
      t_led = 0;
    } else { /* 走行方向SW切替が発生 */
      if (speed == 0) { /* 現在走行速度が0ならば切替 */
        if (dir_next == DIR_NONE) {
          LED_INDICATOR_OFF(); /* ニュートラル */
        } else {
          LED_INDICATOR_ON();  /* 前進/後進*/
        }
        dir_curr = dir_next;
        pp_motor_dir_set(dir_curr);
        t_led = 0;
      } else { /* 走行中の方向切替は禁止: 警告点灯(LED点滅) */
        if (t_led >= T_LED_WARNING) {
          LED_INDICATOR_TOGGLE();
          t_led = 0;
        } else {
          t_led += RUN_INTERVAL;
        }
      }
    }
    /* 加減速度 */
    uint16_t a = pp_controler_accel(target_speed, speed);
    /* 次の走行速度 */
    speed = incr_lin_next(a, SPEED_INC_DY, target_speed);
    /* 走行速度を変換して, モーターPWMデューティー比を得る */
    uint16_t md = PP_CONV_SPEED_TO_MDUTY(speed, p_vmax); 
    /* モーターPWMデューティー比を出力 */
    pp_sb_duty_set(md);
    /* 停車中表示 LED */
    if ((speed == 0) || (dir_curr == DIR_NONE)) {
      LED_STOPPED_ON();
    } else {
      LED_STOPPED_OFF();
    }
  }
  /* 停止 */
  MotorDriver_TB6643KQ_Reset();
  pp_motor_dir_set(DIR_NONE);
  pp_sb_duty_set(0);
  LED_INDICATOR_OFF();
  cmd_reset();
}
