#include <AccelStepper.h>
#include <FastGPIO.h>


#define PIN_VUA_open 12
#define PIN_VDA_open 11
#define PIN_VUB_open 6
#define PIN_VDB_open 5
#define PIN_LSA_CLOSE 10
#define PIN_MOTA_STEP 9
#define PIN_MOTA_DIR 8
#define PIN_LSB_CLOSE 4
#define PIN_MOTB_STEP 3
#define PIN_MOTB_DIR 2

AccelStepper MotA(AccelStepper::DRIVER,PIN_MOTA_STEP,PIN_MOTA_DIR);
AccelStepper MotB(AccelStepper::DRIVER,PIN_MOTB_STEP,PIN_MOTB_DIR);

const int jack_screw = 60 ;  // mm per rev
const int micro_stepping = 4 ; // 
const int motor_resolution = 200 ; // step per rev
const int step_per_mm = motor_resolution * micro_stepping / jack_screw;
const int maxSpeed = 600 * step_per_mm;
const int maxAccel = 600 * step_per_mm;
const int norTravel = 120 * step_per_mm;
const int HomeOFFSET = 15 * step_per_mm;
// const int BHomeOFFSET = 65 * step_per_mm;

int pos[80] = {0,-1,-5,-12,-32,-104,-194,333,1938,4159,
6638,9099,11245,12950,14171,14945,15362,15392,15127,14701,
14269,13962,13798,13747,13754,13831,13936,14063,14209,14394,
14590,14794,14998,15180,15334,15468,15566,15637,15687,15727,
15510,15122,14734,14347,13959,13571,13183,12796,12408,12020,
11632,11245,10857,10469,10081,9694,9306,8918,8530,8143,
7755,7367,6979,6592,6204,5816,5428,5041,4653,4265,
3877,3490,3102,2714,2326,1939,1551,1163,775,388};

int VU[80] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
              1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
              0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
              0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int VD[80] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
              0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
              1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
              1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
              

int LSA_CLOSE = 0;
int LSB_CLOSE = 1;
const bool debug = true;
int run_homeA  = 0;
int run_homeB = 0;
int BPM = 1;
// int sample_period = 1000000 * (60/BPM) / 80 ;
unsigned long sample_period = 20000000 ;
unsigned long nowtime , lasttime ;
int ia = 0;

void Mot_check_LS() {
  LSA_CLOSE = digitalRead(PIN_LSA_CLOSE);
  LSB_CLOSE = digitalRead(PIN_LSB_CLOSE);
  
  if (run_homeA == 1) {  // trying to run home
    // Serial.println("In home loop");
    // Serial.println(LSA_CLOSE);
    MotA.run();
    if (MotA.distanceToGo()==0) {  // end before LS
      if (debug) {Serial.println("Mot_A run Home ERROR!");}
      run_homeA =0;
      MotA.setCurrentPosition(-HomeOFFSET);
    }
    if (LSA_CLOSE==0) {
      run_homeA =0;
      MotA.stop();
      MotA.setCurrentPosition(-HomeOFFSET);
      MotA.setMaxSpeed(maxSpeed);
      MotA.moveTo(pos[0]);
      if (debug) {Serial.println("Mot_A run Home Success"); } 
    }  
  }
  if (run_homeB == 1) {  // trying to run home
    // Serial.println("In home loop");
    // Serial.println(LSA_CLOSE);
    MotB.run();
    if (MotB.distanceToGo()==0) {  // end before LS
      if (debug) {Serial.println("Mot_B run Home ERROR!");}
      run_homeB =0;
      MotB.setCurrentPosition(-HomeOFFSET);
    }
    if (LSB_CLOSE==0) {
      run_homeB =0;
      MotB.stop();
      MotB.setCurrentPosition(-HomeOFFSET);
      MotB.setMaxSpeed(maxSpeed);
      MotB.moveTo(pos[40]);
      if (debug) {Serial.println("Mot_B run Home Success"); } 
    }  
  }
  
  
}

void setup() {
  // put your setup code here, to run once:
  if (debug) {Serial.begin(9600);}
  // power pin for IR receiver
  pinMode(PIN_VUA_open,OUTPUT);
  pinMode(PIN_VDA_open,OUTPUT);
  pinMode(PIN_VUB_open,OUTPUT);
  pinMode(PIN_VDB_open,OUTPUT);
  pinMode(PIN_LSA_CLOSE, INPUT_PULLUP);
  pinMode(PIN_LSB_CLOSE, INPUT_PULLUP);
    // motor driver setting
  MotA.setPinsInverted(PIN_MOTA_DIR);
  MotA.setMaxSpeed(maxSpeed/10);
  MotA.setAcceleration(maxAccel);
  MotA.setCurrentPosition(10000);
  MotB.setPinsInverted(PIN_MOTB_DIR);
  MotB.setMaxSpeed(maxSpeed/10);
  MotB.setAcceleration(maxAccel);
  MotB.setCurrentPosition(10000);
  //delay(2);
  MotA.moveTo(0);
  MotB.moveTo(0);
  run_homeA = 1;
  run_homeB = 1;
  
}

void loop() {
  // put your main code here, to run repeatedly:
  Mot_check_LS();  //blocking when homing
  MotA.run();
  MotB.run();
  //Serial.println("In main loop");


  if (run_homeA + run_homeB ==0 ) {sample_period = 500000;}
  nowtime = micros();
  if (nowtime - lasttime >= sample_period)
  {
    lasttime = nowtime ;  // step forward for next time
    ia++;
    if (ia == 80) {ia = 0;}
    int ib = ia + 40;
    if (ib >= 80) {ib -= 80;}
    MotA.moveTo(pos[ia]);
    MotB.moveTo(pos[ib]);
    digitalWrite(PIN_VUA_open,VU[ia]);
    digitalWrite(PIN_VDA_open,VD[ia]);
    digitalWrite(PIN_VUB_open,VU[ib]);
    digitalWrite(PIN_VDB_open,VD[ib]);
    char buffer[20];
    sprintf(buffer, "%d A: %d %d , B:%d %d\n",ia,VU[ia],VD[ia],VU[ib],VD[ib]);
    Serial.print(buffer);
    FastGPIO::Pin<PIN_VUA_open>::setOutput(VU[ia]);
    FastGPIO::Pin<PIN_VDA_open>::setOutput(VD[ia]);
    FastGPIO::Pin<PIN_VUB_open>::setOutput(VU[ib]);
    FastGPIO::Pin<PIN_VDB_open>::setOutput(VD[ib]);
    
  }
}