/*------------------------------------------------------- 
 * モーター制御(自動運転用)
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "ar.h"
#include "ar-est.h"
#include "incr-lin.h"

static void set_run_params(ar_speedstep_t spdstep_target,
                           ar_decp_id decpi,
                           uint16_t accel,
                           if_sn_id sn_pass,
                           if_sn_id sn_brake,
                           if_sn_id sn_stop);

/* 初期化 */
void ar_motor_init(void) 
{
  ar_motor.speed = 0;
  pp_motor_dir_set(DIR_NONE);
  set_run_params(0, AR_DECP_ID_NULL, 0,
                 IF_SN_NULL, IF_SN_NULL, IF_SN_NULL);
}

/* モータOn (常点灯On) */
void ar_motor_on(void)
{
  pp_motor_dir_set(DIR_FWD);
}

/* モータOff (常点灯Off) */
void ar_motor_off(void)
{
  pp_motor_dir_set(DIR_NONE);
}

/* 走行制御状態機械の走行パラメータ設定:
 * 現在の走行速度から力行・惰行し、停止センサで停車 */
void ar_motor_run_until(ar_speedstep_t spdstep_target,
                        ar_decp_id decpi,
                        if_sn_id sn_brake,
                        if_sn_id sn_stop)
{
  set_run_params(spdstep_target, decpi, p_accel,
                 IF_SN_NULL, sn_brake, sn_stop);
}

/* 走行制御状態機械の走行パラメータ設定: 
 * 現在の走行速度から力行・惰行し、指定センサまで走行(停車しない) */
void ar_motor_run_to(ar_speedstep_t spdstep_target,
                     if_sn_id sn_pass)
{
  set_run_params(spdstep_target, AR_DECP_ID_NULL, p_accel,
                 sn_pass, IF_SN_NULL, IF_SN_NULL);
}

/* 走行制御状態機械の走行パラメータ設定 */
static void set_run_params(ar_speedstep_t spdstep_target,
                           ar_decp_id decpi,
                           uint16_t accel,
                           if_sn_id sn_pass,
                           if_sn_id sn_brake,
                           if_sn_id sn_stop)
{
  /* 走行パラメータを設定 */
  incr_lin_init(ar_motor.speed);
  ar_motor.state = AR_MOTOR_ACC_COA;
  /* 目標速度 (p_vmax によるスケール前) */
  ar_motor.speed_target = AR_SPEEDSTEP_SCALE(spdstep_target);
  /* 減速度 */
  if ((decpi != AR_DECP_ID_NULL) && 
      (ar_decp[decpi].decel == 0)) {
    ar_decp[decpi].decel = p_decel << 2;
  }
  ar_motor.decpi = decpi;
  /* 加速度 */
  ar_motor.accel = accel << 2;
  /* その他各種パラメータ */
  ar_motor.sn_pass  = sn_pass;
  ar_motor.sn_brake = sn_brake;
  ar_motor.sn_stop  = sn_stop;
  ar_motor.spd_brake = 0;
  ar_motor.spd_stop  = 0;
  ar_motor.run_dist = 0;
  ar_motor.snstop_reach = AR_SNSTOP_UNDEF;
  /* センサ値をクリア */
  if_sensor_read();
  if_sensor_get(ar_motor.sn_pass);
  if_sensor_get(ar_motor.sn_brake);
  if_sensor_get(ar_motor.sn_stop);
}

/* 走行制御状態機械 */
int8_t ar_motor_next(void)
{
  /* センサ値 */
  int8_t det_pass  = if_sensor_get(ar_motor.sn_pass);
  int8_t det_brake = if_sensor_get(ar_motor.sn_brake);
  int8_t det_stop  = if_sensor_get(ar_motor.sn_stop);
  /* 減速度パラメータ */
  uint16_t decel = 0;
  if (ar_motor.decpi != AR_DECP_ID_NULL) {
    decel = ar_decp[(uint8_t)ar_motor.decpi].decel;
  }
  if (decel > AR_DECEL_MAX) {
    decel = AR_DECEL_MAX;
  }
  /* 最低維持速度 */
  uint16_t speed_slow = p_vslow;
  /* 目標速度(p_vmaxによる補正後) */
  uint16_t speed_target = 0;
  /* 走行距離の更新値 */
  uint32_t run_dist_next = 0;
  /* 状態遷移 */
  switch (ar_motor.state) {
  case AR_MOTOR_ACC_COA:  /* 力行・惰行 */
    /* 目標速度(p_vmaxによる補正後)を最低維持速度以上に補正 */
    speed_target
      = PP_CONV_SPEED_TO_MDUTY(ar_motor.speed_target, p_vmax);
    if (speed_target < speed_slow) {
      speed_target = speed_slow;
    }
    /* 走行速度 */
    ar_motor.speed
      = incr_lin_next(ar_motor.accel, SPEED_INC_DY, speed_target); 
    /* 事象検査 */
    if ((ar_motor.sn_pass != IF_SN_NULL) &&
        det_pass) { /* 通過センサへ到達 -> 完了(通過) */
      ar_motor.state = AR_MOTOR_DONE;      
    } else if ((ar_motor.sn_brake != IF_SN_NULL) &&
               det_brake) { /* 制動センサへ到達 -> 制動開始 */
      ar_motor.state = AR_MOTOR_BRAKE;
      ar_motor.run_dist = 0;
      ar_motor.spd_brake = ar_motor.speed;
      incr_lin_init(ar_motor.speed);
    }
    break;
  case AR_MOTOR_BRAKE: /* 制動 (制動センサから停止センサへ) */
    /* 走行距離積算 */
    run_dist_next = ar_motor.run_dist + ar_motor.speed; 
    if (ar_motor.run_dist < run_dist_next) { /* オーバーフロー対策 */
      ar_motor.run_dist = run_dist_next; 
    }
    /* 制動 */ 
    ar_motor.speed = incr_lin_next(decel, SPEED_INC_DY, 0);
    if (ar_motor.speed < speed_slow) { /* 速度低下 */
      /* 最低維持速度で走行継続 */
      ar_motor.speed = speed_slow;
    }
    /* 事象検査 */
    if ((ar_motor.sn_stop != IF_SN_NULL) &&
        det_stop) { /* 停止センサへ到達 -> 停車 */
      ar_motor.state = AR_MOTOR_STOP1;
      ar_motor.snstop_reach = AR_SNSTOP_REACH; 
    } else if (ar_motor.speed <= speed_slow) {
      /* 最低維持速度で走行継続 */
      ar_motor.state = AR_MOTOR_SLOW;
      ar_motor.speed = speed_slow;
    }
    break;
  case AR_MOTOR_SLOW: /* 最低維持速度で走行継続 */
    /* 走行距離積算 */
    run_dist_next = ar_motor.run_dist + ar_motor.speed;
    if (ar_motor.run_dist < run_dist_next) { /* オーバーフロー対策 */
      ar_motor.run_dist = run_dist_next;
    }
    /* 事象検査 */
    if ((ar_motor.sn_stop != IF_SN_NULL) &&
        det_stop) { /* 停止センサへ到達 -> 停車 */
      ar_motor.state = AR_MOTOR_STOP1;
      ar_motor.snstop_reach = AR_SNSTOP_UNREACH; 
    } else { /* 最低維持速度で走行継続 */
      ar_motor.speed = speed_slow;
    }
    break;
  case AR_MOTOR_STOP1: /* 減速1(停止センサ到達後) */
    ar_motor.state = AR_MOTOR_STOP2;
    ar_motor.spd_stop = speed_slow;
    incr_lin_init(ar_motor.speed);
    /* 減速度(最低維持速度から停車時の間) */
    uint32_t z = 0;
    if (ar_motor.speed < (speed_slow << 1)) {
      /* 指定時間(AR_TIME_TO_STOP)で停止 */
      z = (uint32_t)AR_TIME_TO_STOP * NRUNS_PER_SEC * SPEED_INC_DY;
    } else if (ar_motor.speed < (speed_slow << 2)) {
      /* 0.25 [Sec] で停止 */
      z = (uint32_t)NRUNS_PER_SEC * SPEED_INC_DY >> 2; 
    } else {
      /* 即座に停止 */
      z = 0; 
    }
    if (ar_motor.speed != 0) {
      ar_motor.decel_stop = z / ar_motor.speed;
    } else {
      ar_motor.decel_stop = z;
    }
    break;
  case AR_MOTOR_STOP2: /* 減速2(停止センサ到達後) */
    /* 事象検査 */
    if (ar_motor.speed > 0) { /* 減速 */
      ar_motor.speed
        = incr_lin_next(ar_motor.decel_stop, SPEED_INC_DY, 0);
    } else { /* 完了(停車) */
      if (ar_motor.snstop_reach == AR_SNSTOP_UNREACH) { 
        /* 停車センサ到達前に速度は最低維持速度へ低下: 減速度が大 */
        ar_estimate_unreached();
      } else if (ar_motor.snstop_reach == AR_SNSTOP_REACH) { 
        /* 速度が最低維持速度に低下する前に停車センサへ到達: 減速度が小 */
        ar_estimate_reached();
      }
      ar_motor.state = AR_MOTOR_DONE;
    }
    break;
  case AR_MOTOR_DONE: /* 完了(停車/通過) */
    break;
  }
  return (ar_motor.state != AR_MOTOR_DONE);
}
