//-----------------------------------------------
//ライブラリ宣言
#include<Stepper.h>
//-----------------------------------------------

//-----------------------------------------------
//変数定義
const int D = 50;                  //プーリー直径
const float STEPANG = 1.8;         //ステップ角
const int WIDTH = 420;             //ステージ横幅
const int T = 3;                   //移動時間
const float TA = 0.1;              //加速時間
const int STEP = 360 / STEPANG;    //一回転のステップ数
const int LIGHT_NUM = 7;           //光受光センサー数
const int LIGHT_PIN[LIGHT_NUM] = {2,3,4,5,6,7,8}; //光受光センサーピン

#define pi 3.14                    //円周率

#define AP 9   //ステッピングモーターピン
#define AM 10  //ステッピングモーターピン
#define BP 11  //ステッピングモーターピン
#define BM 12  //ステッピングモーターピン

#define LIM_SEN1 A0 //原点リミットスイッチ
#define LIM_SEN2 A1 //上限リミットスイッチ
//-----------------------------------------------

//-----------------------------------------------
//ライブラリ変数
Stepper stepm(STEP,AP,AM,BP,BM);
//-----------------------------------------------

//-----------------------------------------------
//パブリック変数
int count = 0;
//-----------------------------------------------

void setup()
{
  Serial.begin(9600);
  
  stepm.setSpeed(30); //ステッピングモーター初期速度

  pinMode(LIM_SEN1,INPUT_PULLUP);
  pinMode(LIM_SEN2,INPUT_PULLUP);

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

  step_set();  //ステッピングモーター原点セット
}

void step_set()
{
  while(1)
  {
    if(digitalRead(LIM_SEN1) == LOW) { break; }
    stepm.step(-1);
    count--;
  }
}

void step_move(int rpm, int stepping, int pm, int light)
{
  int pulse_count = 0;           //パルス数カウント
  int sw = 0;

  stepm.setSpeed(rpm);
  
  while(1)
  {
    if(digitalRead(LIM_SEN1) == LOW && pm < 0) { break; }
    if(digitalRead(LIM_SEN2) == LOW && pm > 0) { break; }
    if(pulse_count == stepping) { break; }
    stepm.step(pm);
    count += pm;
    pulse_count++;

    for(int i = 0; i < LIGHT_NUM; i++)
    { 
      if(digitalRead(LIGHT_PIN[i]) == LOW && (light != i)) { return 0; }
    }
  }
}

void loop()
{
  static float pulse_num = 0.0;    //パルス数
  static float plspeed = 0.0;      //パルス速度
  static float step_po = 0.0;      //ステッピングモーターの現在座標
  static float movdist = 0.0;      //移動距離
  static int rpm = 0;              //回転数
  static int stepping = 0;         //移動距離分のステップ数

  static int light_new = 0;        //受光センサー現在の入力
  static int light_old = 0;        //受光センサー一つ前の入力
  static int pm = 0;               //プラスマイナスカウント

  static float zero_set = (count * STEPANG * pi * D) / 360; //原点
  
  int i = 0;

  while(1)
  { 
    if(i > LIGHT_NUM) { i = 0; }
    if(digitalRead(LIGHT_PIN[i]) == LOW) { light_new = i; break; }
    i++;
  }

  delay(500);

  step_po =  (count * STEPANG * pi * D) / 360 - zero_set;
  movdist = (light_new * 60) - step_po;

  if(movdist != zero_set)
  {
    pulse_num = (abs(movdist) / (D * pi)) * (360 / STEPANG);
    plspeed = pulse_num / (T - TA);

    pm = movdist / abs(movdist);
    rpm = (STEPANG / 360) * plspeed * 60;
    if(rpm == 0) { rpm = 30; }
    stepping = ((abs(movdist) * 360) / (pi * D)) / STEPANG;
    Serial.println(stepping);

    step_move(rpm, stepping, pm, light_new);
  }
}