/*-------------------------------------------------------
 * 主幹制御器による走行制御
 *-------------------------------------------------------*/
#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "mcr-motor.h"
#include "adc.h"
#include "io.h"
#include "pp.h"
#include "pp-const.h"
#include "pp-sb.h"
#include "incr-lin.h"

/* 主幹制御器 */
enum mc_value_e {
  MC_VALUE_NONE,  
  MC_VALUE_BRK3,   /* 制動3 (最強) */
  MC_VALUE_BRK2,   /* 制動2 */
  MC_VALUE_BRK1,   /* 制動1 */
  MC_VALUE_NWT,    /* 緩メ */
  MC_VALUE_ACC1,   /* 力行  */
  MC_VALUE_ACC2,   /* 力行 (最強) */
};

/*-------------------------------------------------------
 * 主幹制御器の位置
 *-------------------------------------------------------*/
static enum mc_value_e read_mc(void)
{
  enum mc_value_e mcv = MC_VALUE_NONE;
  if (SW_MC_BRK3()) {
    mcv = MC_VALUE_BRK3;
  } else if (SW_MC_BRK2()) {
    mcv = MC_VALUE_BRK2;
  } else if (SW_MC_BRK1()) {
    mcv = MC_VALUE_BRK1;
  } else if (SW_MC_NWT()) {
    mcv = MC_VALUE_NWT;
  } else if (SW_MC_ACC1()) {
    mcv = MC_VALUE_ACC1;
  } else if (SW_MC_ACC2()) {
    mcv = MC_VALUE_ACC2;
  }
  return mcv;
}

/*-------------------------------------------------------
 * 主幹制御器の定位置への復帰を待機 (緩メ/制動)
 *-------------------------------------------------------*/
void pp_home_position(void)
{
  uint16_t t = 0;
  uint8_t noks = 0;
  while (noks != 100) { /* 復帰状態が十分な回数連続 */
    __delay_ms(1);
    CLRWDT();
    /* 未復帰: インジケータLEDを点滅 */
    if (t != 0) {
      t--;
    } else {
      t = T_LED_WARNING;
      LED_INDICATOR_TOGGLE();
    }
    /* 緩メ/制動への復帰の検査 (復帰回数を勘定) */
    switch (read_mc()) {
    case MC_VALUE_BRK3: noks++;   break;
    case MC_VALUE_BRK2: noks++;   break;
    case MC_VALUE_BRK1: noks++;   break;
    case MC_VALUE_NWT:  noks++;   break;
    case MC_VALUE_ACC1: noks = 0; break;
    case MC_VALUE_ACC2: noks = 0; break;
    default:
    case MC_VALUE_NONE: noks = 0; break;
    }
    /* モータードライバ LSI TB6643KQ 制御 (過電流検出の解除) */
    if (SW_EMG_STOP()) { /* 解除要求の発生 */
      extern void MotorDriver_TB6643KQ_Reset(void);
      MotorDriver_TB6643KQ_Reset();
    }
  }
  LED_INDICATOR_OFF();
}

/*-------------------------------------------------------
 * 目標走行速度を求める
 *-------------------------------------------------------*/
uint16_t pp_controler_speed(uint16_t speed)
{
  uint16_t s = 0;
  switch (read_mc()) {
  case MC_VALUE_BRK3: s = 0;         break;
  case MC_VALUE_BRK2: s = 0;         break;
  case MC_VALUE_BRK1: s = 0;         break;
  case MC_VALUE_NWT:  s = speed;     break;
  case MC_VALUE_ACC1: s = SPEED_MAX; break;
  case MC_VALUE_ACC2: s = SPEED_MAX; break;
  default:
  case MC_VALUE_NONE: s = speed;     break;
  }
  return s;
}

/*-------------------------------------------------------
 * 加速度&減速度を求める
 *-------------------------------------------------------*/
uint16_t pp_controler_accel(uint16_t target_speed,
                            uint16_t speed)
{
  uint16_t a = 0;
  switch (read_mc()) {
  case MC_VALUE_BRK3: a =  p_decel;       break;
  case MC_VALUE_BRK2: a = (p_decel << 1); break;
  case MC_VALUE_BRK1: a = (p_decel << 2); break;
  case MC_VALUE_NWT:  a =  p_accel;       break;
  case MC_VALUE_ACC1: a = (p_accel << 2); break;
  case MC_VALUE_ACC2: a = (p_accel << 1); break;
  default:
  case MC_VALUE_NONE: a = 4096; break; /* 大きな値(緩やかに加減速) */
  }
  return a;
}
