/*------------------------------------------------------- 
 * トラックプラン: 単線往復
 *-------------------------------------------------------*/
#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. 東駅で停車
 *   3. 東駅を出発し西駅に向かう
 *   4. 西駅で停車
 */
/* 状態 */
#define AR_STATE_GE1      10  /* 西駅→東駅, 走行開始 */
#define AR_STATE_GE2      11  /* 西駅→東駅, 走行継続 */
#define AR_STATE_SE1      12  /* 東駅, 停車開始 */
#define AR_STATE_SE2      13  /* 東駅, 停車継続 */
#define AR_STATE_GW1      14  /* 東駅→西駅, 走行開始 */
#define AR_STATE_GW2      15  /* 東駅→西駅, 走行継続 */
#define AR_STATE_SW1      16  /* 西駅, 停車開始 */
#define AR_STATE_SW2      17  /* 西駅, 停車継続 */
/* 減速パラメータ番号 */
#define AR_DECP_ID_EAST    1  /* 東駅用 */
#define AR_DECP_ID_WEST    2  /* 西駅用 */
/* センサ番号 */
#define AR_SN_EAST_STOP    1  /* S1: 東駅, 停止用 */
#define AR_SN_EAST_BRAKE   2  /* S2: 東駅, 制動用 */
#define AR_SN_WEST_BRAKE   3  /* S3: 西駅, 制動用 */
#define AR_SN_WEST_STOP    4  /* S4: 西駅, 停止用 */

/* 走行制御状態機械 */
enum ar_state_e fsm_shuttle_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_motor_on(); /* モータ＆常点灯On */
    ar_state = AR_STATE_GE1;
    break;
  case AR_STATE_GE1: /* 西駅→東駅 */
    ar_motor_run_until(AR_SPEEDSTEP_MAX, AR_DECP_ID_EAST,
                       AR_SN_EAST_BRAKE, AR_SN_EAST_STOP);
    ar_state = AR_STATE_GE2;
    break;
  case AR_STATE_GE2: /* 走行継続 */
    if (!ar_motor_next()) {
      ar_state = AR_STATE_SE1;
    }
    break;
  case AR_STATE_SE1: /* 東駅, 停車 */
    ar_track1_dir_set(AR_TRACK_DIR_REVERSE); /* 西行 */
    ar_stay_start(AR_STAYTIME_DEFAULT);
    ar_state = AR_STATE_SE2;
    break;
  case AR_STATE_SE2: /* 停車継続 */
    if (!ar_stay_next()) {
      ar_state = AR_STATE_GW1;
    }
    break;
  case AR_STATE_GW1: /* 東駅→西駅 */
    ar_motor_run_until(AR_SPEEDSTEP_MAX, AR_DECP_ID_WEST,
                       AR_SN_WEST_BRAKE, AR_SN_WEST_STOP);
    ar_state = AR_STATE_GW2;
    break;
  case AR_STATE_GW2: /* 走行継続 */
    if (!ar_motor_next()) {
      ar_state = AR_STATE_HOME;
    }
    break;
  case AR_STATE_HOME:   /* 定位置(西駅) */
    ar_state = AR_STATE_SW1;
    break;
  case AR_STATE_SW1: /* 西駅, 停車 */
    ar_track1_dir_set(AR_TRACK_DIR_NORMAL); /* 東行 */
    ar_stay_start(AR_STAYTIME_DEFAULT);
    ar_state = AR_STATE_SW2;
    break;
  case AR_STATE_SW2: /* 停車継続 */
    if (!ar_stay_next()) {
      ar_state = AR_STATE_GE1;
    }
    break;
  }
  return ar_state;
}
