/*------------------------------------------------------- 
 * トラックプラン: 単線エンドレス
 *-------------------------------------------------------*/
#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. 一周してきて駅で停車
 */
/* 状態 */
#define AR_STATE_RUN1    10  /* 東に向かい駅を出発, 走行開始 */
#define AR_STATE_RUN2    11  /* 走行継続 */
#define AR_STATE_STA1    12  /* 駅, 停車 */
#define AR_STATE_STA2    13  /* 駅, 停車継続 */
/* 減速パラメータ番号 */
#define AR_DECP_ID_STA    1  /* 駅用 */
/* センサ番号 */
#define AR_SN_STA_STOP    1  /* S1: 駅, 停止用 */
#define AR_SN_STA_BRAKE   2  /* S2: 駅, 制動用 */

/* 走行制御状態機械 */
enum ar_state_e fsm_endless_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_RUN1;
    break;
  case AR_STATE_RUN1: /* 駅を出発 */
    ar_motor_run_until(AR_SPEEDSTEP_MAX, AR_DECP_ID_STA,
                       AR_SN_STA_BRAKE, AR_SN_STA_STOP);
    ar_state = AR_STATE_RUN2;
    break;
  case AR_STATE_RUN2: /* 走行継続 */
    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_stay_start(AR_STAYTIME_DEFAULT);
    ar_state = AR_STATE_STA2;
    break;
  case AR_STATE_STA2: /* 駅, 停車継続 */
    if (!ar_stay_next()) {
      ar_state = AR_STATE_RUN1;
    }
    break;
  }
  return ar_state;
}
