/*------------------------------------------------------- 
 * トラックプラン: リバース
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "pp.h"
#include "pp-const.h"
#include "ar.h"
#include "ar-motor.h"
#include "fsm.h"

/*
 *　　北　　　　　　　　　　　　　Ｓ３
 *　西＋東　　　　　　　　　　　＝＃＝　
 *　　南　　　　　　　　　　　／／　　＼＼
 *　　　　　　　　　　　　　｜｜　　　　｜｜
 *　　　　　　　　　　　　　ＸＸ　　　　｜｜
 *　　　駅　　　　Ｆ１　　／／　　　　／／
 *　＝＃＝＝＃＝＝↑＝＝＊＝＝ＸＸ＝↑
 *　　Ｓ２　Ｓ１　　　　Ｔ１　　　　Ｆ２
 *　（Ｆ：フィーダ、Ｓ：センサ、Ｔ：ポイント、ＸＸ：ギャップ）
 *
 * 動作シーケンスの概要(以下の繰り返し): 
 *   1. 駅を出発
 *   2. エンドレス区間の途中(S3)でポイントT1を切替
 *   3. 駅に停車
 */
/* 状態 */
#define AR_STATE_RUN1    10 /* 駅を出発, 走行開始 */
#define AR_STATE_RUN2    11 /* 走行継続 */
#define AR_STATE_RUN3    12 /* リバース線処理 */
#define AR_STATE_RUN4    13 /* 走行継続 */
#define AR_STATE_STA1    14 /* 駅, 停車 */
#define AR_STATE_STA2    15 /* 駅, 停車継続 */
#define AR_STATE_STA3    16 /* 駅, 停車終了 */
/* 減速パラメータ番号 */
#define AR_DECP_ID_STA    1  /* 駅用 */
/* センサ番号 */
#define AR_SN_STA_BRAKE   1  /* S1: 駅, 制動用 */
#define AR_SN_STA_STOP    2  /* S2: 駅, 停止用 */
#define AR_SN_REVERSE     3  /* S3: リバース区間進入検知用 */
/* ポイント番号 */
#define AR_TO_REVERSE     1  /* T1: リバース用 */

/* 走行制御状態機械 */
enum ar_state_e fsm_reverse_next(enum ar_state_e ar_state)
{
  /* 初期状態: 駅に停車 */
  switch ((uint8_t)ar_state) {
  default:
    ar_state = AR_STATE_INIT;
    break;
  case AR_STATE_INIT:
    ar_track1_dir_set(AR_TRACK_DIR_NORMAL);
    ar_track2_dir_set(AR_TRACK_DIR_NORMAL);
    ar_motor_on(); /* モータ＆常点灯On */
    if_toq_set_normal(AR_TO_REVERSE);
    ar_state = AR_STATE_RUN1;
    break;
  case AR_STATE_RUN1: /* 駅を出発 (リバース区間センサまで) */
    ar_motor_run_to(AR_SPEEDSTEP_MAX, AR_SN_REVERSE);
    ar_state = AR_STATE_RUN2;
    break;
  case AR_STATE_RUN2: /* 走行継続 */
    if (!ar_motor_next()) {
      ar_state = AR_STATE_RUN3;
    }
    break;
  case AR_STATE_RUN3: /* リバース線処理: 極性反転&ポイント切替 */
    ar_track1_dir_set(AR_TRACK_DIR_REVERSE);
    if_toq_set_reverse(AR_TO_REVERSE);
    /* 駅に向けて走行 */
    ar_motor_run_until(AR_SPEEDSTEP_MAX, AR_DECP_ID_STA,
                       AR_SN_STA_BRAKE, AR_SN_STA_STOP);
    ar_state = AR_STATE_RUN4;
    break;
  case AR_STATE_RUN4: /* 走行継続 */
    if (!ar_motor_next()) {
      ar_state = AR_STATE_HOME;
    }
    break;
  case AR_STATE_HOME:   /* 定位置(駅) */
    ar_state = AR_STATE_STA1;
    break;
  case AR_STATE_STA1: /* 駅, 停車 */
    ar_track1_dir_set(AR_TRACK_DIR_NORMAL);
    ar_stay_start(AR_STAYTIME_DEFAULT);
    ar_state = AR_STATE_STA2;
    break;
  case AR_STATE_STA2: /* 停車継続 */
    if (!ar_stay_next()) {
      ar_state = AR_STATE_STA3;
    }
    break;
  case AR_STATE_STA3: /* 停車終了 */
    if_toq_set_normal(AR_TO_REVERSE); /* 定位 */
    ar_state = AR_STATE_RUN1;
    break;
  }
  return ar_state;
}
