/*-------------------------------------------------------
 * 自動運転モード
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "pp-sb.h"
#include "ar.h"
#include "fsm.h"
#include "if-toq.h"
#include "if.h"

/* 走行制御状態機械 */
struct ar_motor_s ar_motor;
/* 自動運転状態 */
static enum ar_state_e ar_state = AR_STATE_INIT;
/* 自動運転パラメータ値 */
uint16_t  p_vslow   = 0;
uint16_t  p_staytim = 0;

static void wait_start_button_release(void);

/* 各種パラメータ読み込み */
void ar_param_read(void)
{
  /* 標準パラメータを読み込む */
  pp_param_read();
  /* ボリューム値の読み込み */
  uint16_t vr_vslow   = pp_adc_read(ADC_CH_VSLOW);
  uint16_t vr_staytim = pp_adc_read(ADC_CH_STAYTIM);
  /* 割り込み禁止 */ 
  int8_t ogie  = GIE;
  int8_t opeie = PEIE;
  GIE  = 0;
  PEIE = 0;
  /* パラメータ値の決定 */
  p_vslow   = vr_vslow >> 1; /* 0 ..  511 */
  p_staytim = vr_staytim;    /* 0 .. 1023 */
  /* 割り込み禁止/許可状態を復元 */ 
  GIE  = ogie;
  PEIE = opeie;
}

/* 線路極性設定(トラック1) */
void ar_track1_dir_set(enum ar_track_dir_e tdir)
{
  if (tdir == AR_TRACK_DIR_NORMAL) {
    TDIR1_NORM();
  } else {
    TDIR1_REV();
  }
}

/* 線路極性設定(トラック2) */
void ar_track2_dir_set(enum ar_track_dir_e tdir)
{
  if (tdir == AR_TRACK_DIR_NORMAL) {
    TDIR2_NORM();
  } else {
    TDIR2_REV();
  }
}

/* 走行開始ボタンの開放を待機 */
static void wait_start_button_release(void)
{
  uint8_t c = 0;
  /* 一定回数連続したSTARTボタンの解放を検出(チャタリング対策) */
  while (c != 100) { 
    pp_delay_ms(1);
    CLRWDT();
    if (SW_START()) {
      c = 0; /* 押下中 */
    } else {
      c++;   /* 開放中 */
    }
  }
}

/* 自動運転初期化 */
void auto_run_init(void)
{
  /* センサ状態の初期化 */
  if_sensor_reset();
  /* 減速度パラメータの初期化 */
  ar_param_read(); 
  ar_decp_init();
}

/* 自動運転開始 */
void auto_run_start(void)
{
  /* LED */
  LED_RUN_OFF();
  LED_IDLE_ON();
  LED_SENS_OFF();
  /* 自動運転開始ボタンの押下を待機 */
  while (!SW_START()) {
    CLRWDT();
    /* 現在モード終了 */
    if (!SW_MODE_AUTO()) {
      return;
    }
    /* センサ検出 LED 表示 */
    if_sensor_read();
    if (if_sensor_detected()) {
      LED_SENS_ON();
    } else {
      LED_SENS_OFF();
    }
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { /* 解除要求の発生 */
      extern void MotorDriver_TB6643KQ_Reset(void);
      MotorDriver_TB6643KQ_Reset();
    }
  }
  /* 自動運転開始ボタンの開放を待機 */
  wait_start_button_release();
  /* LED表示(全On) */
  LED_RUN_ON();
  LED_IDLE_ON();
  LED_SENS_ON();
  /* 自動運転モーター制御の初期化 */
  ar_motor_init();
  /* 全ポイント定位 */
  if_toq_set_all_reverse(); /* 全反位 */
  while (if_toq_next(RUN_INTERVAL)) {
    pp_delay_ms(RUN_INTERVAL);
    CLRWDT();
  }
  if_toq_set_all_normal();  /* 全定位 */
  while (if_toq_next(RUN_INTERVAL)) {
    pp_delay_ms(RUN_INTERVAL);
    CLRWDT();
  }
  /* LED表示(全Off) */
  LED_RUN_OFF();
  LED_IDLE_OFF();
  LED_SENS_OFF();
  /* オートオフ用タイマ */
  uint32_t  t_auto_off_grace = 0;
  uint32_t  t_auto_off_brute = 0;
  /* 終了予告 LED タイマ */
  uint16_t  t_led_stop = 0;
  /* 終了フラグ(定位置にて) */
  int8_t stop_gracefully = 0;
  /* 主ループ(自動運転) */
  ar_state = AR_STATE_INIT;
  for (;;) {
    pp_delay_ms(RUN_INTERVAL);
    CLRWDT();
    /* RUN LED 点滅 */
    static uint8_t t_led_tick = 0;
    t_led_tick++;
    if (t_led_tick == NRUNS_PER_SEC) {
      LED_RUN_ON();
      t_led_tick = 0;
    } else {
      LED_RUN_OFF();
    }
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { /* 解除要求の発生 */
      extern void MotorDriver_TB6643KQ_Reset(void);
      MotorDriver_TB6643KQ_Reset();
    }
    /* 非常停止 */
    if (SW_EMG_STOP()) {
      break;
    }
    /* 現在モード終了 */
    if (!SW_MODE_AUTO()) {
      break;
    }
    /* 終了ボタン押下チェック */
    if (SW_END()) {
      stop_gracefully = 1;
    }
    /* オートオフ処理 */ 
    if (t_auto_off_grace < T_AUTO_OFF) {
      t_auto_off_grace += RUN_INTERVAL;
      t_auto_off_brute = 0;
    } else {
      stop_gracefully = 1; /* 列車が定位置に来れば終了 */
      if (t_auto_off_brute < T_AUTO_OFF_BRUTE) {
        t_auto_off_brute += RUN_INTERVAL;
      } else {
        break; /* 強制終了(定位置にあるか否かに関わらず) */
      }
    }
    /* 終了予告: IDLE LED 点滅 */
    if (stop_gracefully) {
      t_led_stop += RUN_INTERVAL;
      if (t_led_stop > T_LED_WARNING) {
        t_led_stop = 0;
        LED_IDLE_TOGGLE();
      }
    }
    /* センサ読み込み */
    if_sensor_read();
    /* センサ検出 LED 点灯 */
    if (if_sensor_detected()) {
      LED_SENS_ON();
    } else {
      LED_SENS_OFF();
    }
    /* 自動運転の状態遷移 */
    ar_state = fsm_next(ar_state);
    /* 終了(列車は定位置) */
    if ((ar_state == AR_STATE_HOME) && stop_gracefully) { 
      break;
    }
    /* ポイント切替 */
    if_toq_next(RUN_INTERVAL);
    /* ボリューム読み込み */ 
    ar_param_read(); 
    /* 現在走行速度 */
    uint16_t speed = ar_motor.speed;
    /* 走行速度を変換して, モーターPWMデューティー比を得る */
    uint16_t md = speed;
    /* モーターPWMデューティー比を出力 */
    pp_sb_duty_set(md);
  }
}
