#include <AccelStepper.h>
/*
 * stepper control through big easy driver
 * positions are expressed in centimeters
 * originPos is near the motor, where switchLow gets LOW
 * maxPos is the maximum position, where switchHigh gets LOW
 * idlePos is the idle position
 * lowOffset 
 * highOffset are offsets from originPos/maxPos respectively to define the running zone
 * 
 * Stepper driven through big easy driver
 * configured for 1/4 microstepping (mover micro-steps gives low speed)
 * 800 steps is one rotation
 * one rotation is 1.6cm translation
 * cm to steps ratio is 500 
 */

 // Define a stepper and the pins it will use
const int NUM_OF_STEPPERS = 2 ;
AccelStepper steppers[NUM_OF_STEPPERS] = {AccelStepper(1, 8, 7), AccelStepper(1, 10, 9)}; ;

// Mechanical values
long int STEPS_PER_TURN = 200 ; // stepper motor
int MICROSTEPPING = 8 ; //stepper driver
int REDUCTION_RATIO = 70 ;  // mechanical reduction ration to real camera rotation

// microswitch pins
int switchLow = A5;

int driveMode ; // global stepper driving mode : default position with acceleration, 1 position with constant speed, 2 constant speed
long int speedLimit[2] = { 50 , 10000000 } ;// sets an absolute minimum/maximum speed limit
long int accLimit[2] = { 100, 200000} ;

//////////// UTILITIES  ///////////////


long int camRot2steps (float camRot ) {
  return (long int) STEPS_PER_TURN * MICROSTEPPING * REDUCTION_RATIO *camRot ;
}



////////////  ACCELSTEPPER  /////////////

void stop_Steppers (void) {
  for(int i ; i < NUM_OF_STEPPERS ; i++ ) {
    steppers[i].stop();
  }
  
}

long int moveTo_Stepper(int stepperIndex, long int stepto ) {
  steppers[stepperIndex].moveTo(stepto);
//  while (!steppers[stepperIndex].distanceToGo()==0)
//    steppers[stepperIndex].run();
//  steppers[stepperIndex].moveTo( constrain( stepto , originPos * stepToCm, maxPos * stepToCm ) );
  return steppers[stepperIndex].distanceToGo();
}
//float moveToCm_Stepper(float cmto ) {
//  stepper.moveTo(constrain( cmto * stepToCm , originPos, maxPos ));
//  return stepper.distanceToGo()/stepToCm;
//}
//
long int move_Stepper(int stepperIndex, long int stepRelative ) {
  steppers[stepperIndex].move(stepRelative);
  return steppers[stepperIndex].distanceToGo();
}
//float moveCm_Stepper(float cmRelative ) {
//  stepper.move(cmRelative * stepToCm );
//  return stepper.distanceToGo()/stepToCm;
//}
//
long int maxSpeed_Stepper(int stepperIndex, long int maxspeed){
  steppers[stepperIndex].setMaxSpeed( constrain (maxspeed, speedLimit[0], speedLimit[1] ) );
  return steppers[stepperIndex].maxSpeed();
}

long int setSpeed_Stepper(int stepperIndex, long int maxspeed){
  steppers[stepperIndex].setSpeed( constrain (maxspeed, speedLimit[0], speedLimit[1] ) );
  return steppers[stepperIndex].maxSpeed();
}

long int acceleration_Stepper(int stepperIndex, long int acc ){
  steppers[stepperIndex].setAcceleration(acc + 1); // prevent 0 step/s²
  return acc;
}



//////////////// CALIBRATION ////////////////////////

float init_steppers(){
//  //run at constant speed until hitting switchLow
//  stepper.setSpeed(-1*speedLimit[1]); 
//  while(digitalRead(switchLow)) {
//    stepper.runSpeed();
//  }
//  stepper.stop();
//  //this is the new 0 step
//  stepper.setCurrentPosition(0);
//
//
//
//  delay(1000); //take a breath
//
//  //run at constant speed until hitting switchHigh
//  stepper.setSpeed(speedLimit[1]);
//  while(digitalRead(switchHigh)) {
//    stepper.runSpeed(); 
//  }
//  stepper.stop();
//  maxPos = stepper.currentPosition() / stepToCm ;
//
//  delay(1000);
//
//  stepper.runToNewPosition(maxPos * stepToCm / 2) ; 
//  return maxPos ;
}

///////////////////// SETUP ///////////////////////////
void setup_steppers(){
  pinMode (switchLow, INPUT_PULLUP);
//  pinMode (switchHigh, INPUT_PULLUP);
//  stepper.moveTo(rand() % 200000);
  for(int i ; i < NUM_OF_STEPPERS ; i++ ) {
    steppers[i].setMaxSpeed( 20000);
    steppers[i].setSpeed(10000);
  }
//  while (!stepper.distanceToGo()==0)
//    stepper.run();
//  stepper.moveTo(0);
//  while (!stepper.distanceToGo()==0)
//    stepper.run();
//  stepper.setMaxSpeed(speedLimit[1]);
//  //stepper.setSpeed(10000);
//  stepper.setAcceleration(accLimit[1]);
  
}

///////////////////// LOOP ///////////////////////////

bool update_steppers() {

  for(int i ; i < NUM_OF_STEPPERS ; i++ ) {
    switch (driveMode) {
      case 1:
        steppers[i].runSpeedToPosition();
        break ;
      case 2:
        steppers[i].runSpeed();
        break ;
      default :
        steppers[i].run();
    }
      
        
  }
  
//  bool updated = 1 ;

  
  
//  if (!digitalRead(switchLow)) {
//    stepper.setCurrentPosition(0);
//    updated=0;
//  }
//  if (!digitalRead(switchHigh)) {
//    stepper.setCurrentPosition(maxPos * stepToCm);
//    updated=0;
//  }
//  return updated ;
}