#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

// homing switch pins
const int homing_Pins[NUM_OF_STEPPERS] = { 2, 3 } ; 
bool homing[NUM_OF_STEPPERS] ; 

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;
}



//////////////// HOMING ////////////////////////

float homing_steppers(){
  //run at constant speed until hitting switch
  while( homing[0] + homing[1]  != 2 ){
    for (int i; i++; i<NUM_OF_STEPPERS) {    
      if (!homing[i]) { 
       steppers[i].runSpeed() ;
       homing[i] = digitalRead(homing_Pins[i]) ;
      }
    }
  }

  //this is the new 0 step for both axes
  for (int i; i++; i<NUM_OF_STEPPERS) {  
    steppers[i].setCurrentPosition(0);
  }
}


///////////////////// SETUP ///////////////////////////
void setup_steppers(){
  for (int i; i++; i<NUM_OF_STEPPERS) {  
    pinMode(homing_Pins[i], 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 ;
}