//-------------------------------------------------------------------------
//ライブラリ定義
#include <math.h>                                 //数学関数lib
#include <MsTimer2.h>                             //タイマーlib
//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//マクロ定義
#define LOG(x, y) (log(y) / log(x))               //log関数
//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//define変数定義

//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//ピン定義
const int light_sensor_pin[7] = { 0, 1, 2, 3, 4, 5, 6 };    //光センサピン
const int ls_l_pin = 8;                                     //LS左ピン
const int ls_r_pin = 9;                                     //LS右ピン

const int RESET = 13;                                       //リセットピン
const int CW = 12;                                          //cw・ccwピン
const int CLOCK = 11;                                       //クロックピン
const int SLEEP = 10;                                       //スリープピン
//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//パラメータ特性定義
const int R = 50;                                 //プーリ直径
const float step_ang = 1.8;                       //ステップ角
const float pi = 3.141592653589;                  //円周率
const float SM_val = R * pi * step_ang;           //SM特性

const int max_hz = 200;                           //最大動作周波数
const float tb_per = 0.8;                         //定速時間割合
const float tac_per = 0.1;                        //加減速時間割合
//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//タイマー関数変数定義
volatile int light_sensor = 0;                    //光センサデータ
volatile int light_sensor_old = 0;                //光センサ前入力
volatile int step_flag = 0;                       //立ち上がりでステップ終了
//-------------------------------------------------------------------------

//-------------------------------------------------------------------------
//グローバル変数定義
int SM_count = 0;                       //SMパルスカウント
//-------------------------------------------------------------------------

int LS_L_LIM() { return digitalRead(ls_l_pin) ^ 1; }
int LS_R_LIM() { return digitalRead(ls_r_pin) ^ 1; }
int LS_LIM() { return LS_L_LIM() | LS_R_LIM(); }

void SM_step(int hz, int rotate) {
  uint32_t delay_t = 0;                           //delay用変数
  uint32_t m_delay_t = 0;                         //delayMicro用変数
  uint32_t SM_T = 0.0;                            //周期
  
  hz = (hz > 0) ? hz : 1;
  SM_T = (1.0 / hz) * pow(10 ,6);
  delay_t = SM_T / 1000;
  m_delay_t = SM_T % 1000;

  if(rotate == -1) { digitalWrite(CW, LOW); }
  if(rotate == 1) { digitalWrite(CW, HIGH); }

  digitalWrite(CLOCK, HIGH);
  if(delay_t > 0) { delay(delay_t / 2); }
  if(m_delay_t > 0) { delayMicroseconds(m_delay_t / 2); }

  digitalWrite(CLOCK, LOW);
  if(delay_t > 0) { delay(delay_t / 2); }
  if(m_delay_t > 0) { delayMicroseconds(m_delay_t / 2); }

  SM_count += rotate;
}

void sensor_read() {
  int sensor_temp = 0;                            //センサデータ仮格納
  static int pulse_up = 0;                        //センサ立ち上がり検出
  static int sensor_tar = 0;                      //センサ目標番号
  
  sensor_temp = LOG(2,(PIND << 1) ^ 0b11111110);
  if(sensor_temp > 0 && pulse_up == 0) {
    if(sensor_tar != sensor_temp) {
      light_sensor = sensor_temp - 1;
      light_sensor_old = sensor_temp;
      sensor_tar = sensor_temp;
      pulse_up = 1;
      step_flag = 1;
    }
  }else {
    if(pulse_up == 1 && sensor_temp != light_sensor_old) {
      pulse_up = 0;
      light_sensor_old = sensor_temp;
    }
  }
}

void setup() {
  int i;

  for(i = 0; i < 7; i++) {
    pinMode(light_sensor_pin[i], INPUT_PULLUP);
  }

  pinMode(ls_l_pin, INPUT_PULLUP);
  pinMode(ls_r_pin, INPUT_PULLUP);

  pinMode(RESET,OUTPUT);
  pinMode(CW,OUTPUT);
  pinMode(CLOCK,OUTPUT);
  pinMode(SLEEP,OUTPUT);

  MsTimer2::set(10, sensor_read);
  MsTimer2::start();
}

void loop() {
  float SM_pos = 0;                         //SM座標
  float SM_move = 0;                        //SM移動量
  float SM_pulse = 0;                       //SMパルス数
  int SM_cw = 0;                            //回転方向

  int ta_count = 0;                         //台形制御加速時間カウント
  int tb_count = 0;                         //台形制御定速時間カウント
  int tc_count = 0;                         //台形制御減速時間カウント

  int loop_count = 1;                       //while内カウント変数
  int hz = 0;                               //動作周波数

  SM_pos = (SM_count * SM_val) / 360;
  SM_move = (light_sensor * 60) - SM_pos;
  SM_pulse = (abs(SM_move) * 360) / SM_val;
  SM_cw = SM_move / abs(SM_move);

  ta_count = round(SM_pulse * tac_per);
  tb_count = round(SM_pulse * tb_per);
  tc_count = round(SM_pulse * tac_per);
  
  //加速期間
  loop_count = 1;
  while(loop_count <= ta_count && step_flag == 0) {
    if(LS_LIM() == 1) break;
    hz = loop_count * max_hz / ta_count;
    SM_step(hz, SM_cw);
    loop_count++;
  }

  //定速期間
  loop_count = 1;
  while(loop_count <= tb_count && step_flag == 0) {
    if(LS_LIM() == 1) break;
    hz = max_hz;
    SM_step(hz, SM_cw);
    loop_count++;
  }

  //減速期間
  loop_count = tc_count;
  while(loop_count > 0 && step_flag == 0) {
    if(LS_LIM() == 1) break;
    hz = loop_count * max_hz / tc_count;
    SM_step(hz, SM_cw);
    loop_count--;
  }
  step_flag = 0;
  delay(500);
}