//New Stepper V35

//global system pins
const int openPin = 2; //open button M1 + M2
const int closePin = 3; //close button M1 + M2
const int openDisablePin = 10; // disable open movement M1 + M2
const int closeDisablePin = 11; // disable close movement M1 + M2
const int ledPin = 13; //signal validation monitor pin

//M1 pins
const int stepPinM1 = 4; //step pin motor 1
const int dirPinM1 = 5; //direction pin motor 1 (0 CLOCKWISE, 1 COUNTER LOCKWISE)
const int openLimitPinM1 = 6; //inductive open sensor M1
const int closeLimitPinM1 = 7; //inductive close sensor M1
const int adjustOpenLimitPinM1 = 8; //open adjustment M1
const int adjustCloseLimitPinM1 = 9; //close adjustment M1

//M2 pins
const int stepPinM2 = A0; //step pin motor 2
const int dirPinM2 = A1; //direction pin motor 2 (0 CLOCKWISE, 1 COUNTER LOCKWISE)
const int openLimitPinM2 = A2; //inductive open sensor M2
const int closeLimitPinM2 = A3; //inductive close sensor M2
const int adjustOpenLimitPinM2 = A4; // open adjustment M2
const int adjustCloseLimitPinM2 = A5; // close adjustment M2

//arrays
#define numSignals 12
// INDEX..............................[0].......[1].........[2]...............[3].............[p4]...........[5]..................[6].................[7]...................[8]............[9]..............[10]...................[11]............
const int signalPins[numSignals] = {openPin, closePin, openDisablePin, closeDisablePin, openLimitPinM1, closeLimitPinM1, adjustOpenLimitPinM1, adjustCloseLimitPinM1, openLimitPinM2, closeLimitPinM2, adjustOpenLimitPinM2, adjustCloseLimitPinM2};
boolean signalValidation[numSignals]; //validation output (1=valid)
boolean signalState[numSignals];        // current state of the button
boolean lastSignalState[numSignals];    // previous state of the button
int highSignalValidationCounter[numSignals]; // signal counter for valid open or close signals
int lowSignalValidationCounter[numSignals]; // signal counter for valid open or close signals

//global input variables
double time = 5; // open time in seconds (manual input)
double pulleyPace = 5;  // pace per tooth in mm for largest pulley (manual input)
double numberTeethPulley = 18;  //teeth per pulley for largest pulley (manual input)
int stepPerRevolution = 400; //number of pulses per revolution M1 - 2 steps is one rev step (manual input)
double accelerationRamp = 20; //percentage of set time to accelerate and decelerate

//global system variables
int N; //number of steps in acceleration to maximum speed
int systemState; //operation state (0=oppening, 1=closing, 2=openned, 3=closed, 4=partial)
double currentMicros;  //the current time in microseconds (begins count at power on)
double currentMillis; //the current time in milliseconds (begins count at power on)
double rampTime; // micros to change speed
double distancePerRev; // (mm) linear distance per revolution
double L; //linear distance for acceleration to maximum speeed (mm)
double R; //linear length of one step (mm)
double T; //total time for accceleration to maximum speed (s)
double Vmax; //maximum linear speed (mm/s)
double d; //delay in acceleration (s)
double f; //times in acceleration (s)
double lastTd;// Last time found. Used to calculate delay
unsigned long lastDebounceTime;  // (millis) the last time the output pin was toggled
unsigned long lastDebugTime;
unsigned long debounceDelay;    // (millis) the debounce time; increase if the output flickers

//M1 input variables
double distanceM1 = 2800; // linear distance in mm (manual input)
double openAdjustmentM1 = 1;// percentage of steps to move and increase openTarget
double closeAdjustmentM1 = 1; // percentage of steps to move and increase closeTarget

//M1 system variables
boolean nextStepM1; //togles pulses for stepPinM1
int stepPositionM1; //gate position in steps
int positionM1; //gate position as percentage of open (100% = totally open)
double stepMicrosM1;  //timing of M1 movement (begins count if openPin==1 or closePin)
int stepCountM1; //global count
int stepTargetM1;  // set total theoretical steps to open and close (calculation based on manual inputs)
double openStepTargetM1;  // set total open steps
double closeStepTargetM1;  // set total close steps
double stepDelayM1;  //micros betwen steps for M1 (amplitude - larger decreases speed)
double stepLenghtM1; // mm per step
double revolutionTargetM1; // set total theoretical revolutions
double RPMM1; // set rpm
double angularSpeedM1;
double maxLinearSpeedM1; // set linear speed
double linearSpeedM1; // set linear speed
double linearAccelerationM1;
double angularAccelerationM1;
double accelerationRampDistanceM1;
double accelerationRampRevolutionsM1;
double accelerationRampStepsM1;
double stepDelayAccelerationM1;

/*double stepAngleM1;
  double positionM1;
  double speedM1;
  double accelerationM1;
  double initialDelayM1;
  double constantDelayM1;
  double stepsM1;
  double accelerationStepsM1,
  double accelerationLimitStepsM1,
  double decelerationStepsM1,
*/

//M2 input variables
double distanceM2 = 2800; // linear distance in mm (manual input)
double openAdjustmentM2 = 1;// percentage of steps to move and increase openTarget
double closeAdjustmentM2 = 1; // percentage of steps to move and increase closeTarget

//M2 system variables
boolean nextStepM2; //togles pulses for stepPinM2
int stepPositionM2; //gate position in steps
int positionM2; //gate position as percentage of open (100% = totally open)
double stepMicrosM2;  //timing of M2 movement (begins count if openPin==1 or closePin)
int stepCountM2; //global count
int stepTargetM2;  // set total theoretical steps to open and close (calculation based on manual inputs)
double openStepTargetM2;  // set total open steps
double closeStepTargetM2;  // set total close steps
double stepDelayM2;  //micros betwen steps for M2 (amplitude - larger decreases speed)
double revolutionTargetM2; // set total theoretical revolutions
double RPMM2; // set rpm
double angularSpeedM2;
double linearSpeedM2; // set linear speed
double linearAccelerationM2;
double angularAccelerationM2;
double accelerationRampDistanceM2;
double accelerationRampRevolutionsM2;
double accelerationRampStepsM2;
double stepDelayAccelerationM2;


//functions

//signal validation
int signalVal () {
  // debounce
  if ((currentMillis - lastDebounceTime) > debounceDelay) {
    // read the signal input pin
    for (int i = 0; i < numSignals ; i++) {
      signalState[i] = digitalRead(signalPins[i]);
      // compare the signalState to its previous state
      if (signalState[i] != lastSignalState[i]) {
        // if the current state is HIGH then the signal went from off to on
        if (signalState[i] == HIGH) {
          highSignalValidationCounter[i]++;
          digitalWrite(ledPin, HIGH);
          signalValidation[i] = 1;
          //String signalPn = "signalPin " + String (signalPins[i]) + " ON  " + String (signalValidation[i]) + "HighCounter " + String (highSignalValidationCounter[i]);
          //Serial.println(signalPn);
        }
        else {
          // if the current state is LOW then the signal went from on to off
          lowSignalValidationCounter[i]++;
          digitalWrite(ledPin, LOW);
          signalValidation[i] = 0;
          //String signalPn = "signalPin " + String (signalPins[i]) + " OFF " + String (signalValidation[i]) + "LowCounter " + String (lowSignalValidationCounter[i]);
          //Serial.println(signalPn);
        }
      }
      // save the current state as the last state
      lastSignalState[i] = signalState[i];
    }
    // reset the debouncing timer
    lastDebounceTime = currentMillis;
  }
}

//state evaluation and commands
void stateEval () {
  if (systemState == 0) { //check state (openning)
    openStepControlM1();
    if (signalState[2] == 1 || highSignalValidationCounter[0] > 0 || highSignalValidationCounter[1] > 0 || stepCountM1 >= openStepTargetM1) { //check impediments to open
      systemState = 4; // stop oppening motion set state to into partial
      highSignalValidationCounter[0] = 0; //reset open button count
      highSignalValidationCounter[1] = 0; //reset close button count
    }
    else if (signalState[4] == 1) { //check open limit signal
      systemState = 2;//stop movement and change state to open
      openStepTargetM1 = stepCountM1; //reset closeStepCounter
    }
  }
  else if (systemState == 1) {//check state (closing)
    closeStepControlM1();
    if (signalState[3] == 1 || highSignalValidationCounter[0] > 0 || highSignalValidationCounter[1] > 0 || stepCountM1 <= closeStepTargetM1) {//check impediments to close
      systemState = 4; //stop closing motion set state to into partial
      highSignalValidationCounter[0] = 0;
      highSignalValidationCounter[1] = 0;
    }
    else if (signalState[5] == 1) { //check close limit signal
      systemState = 3;//stop movement and change state to closed
      stepCountM1 = 0; //reset step count
    }
  }
  else if (systemState == 2) {//check state (openned)
    highSignalValidationCounter[0] = 0; //ignore open command signal
    if (signalState[3] == 0 ) { //check close disable signal
      if (highSignalValidationCounter[1] > 0) { //check close command signal
        systemState = 1;//set close motion change status to closing
        highSignalValidationCounter[1] = 0;
      }
    }
  }
  else if (systemState == 3) {//check state (closed)
    highSignalValidationCounter[1] = 0; //ignore close command signal
    if (signalState[2] == 0 ) { //check open disable signal
      if (highSignalValidationCounter[0] > 0) {//check open command signal
        systemState = 0;//set open motion change status to oppening
        lastTd = 0.0;
        highSignalValidationCounter[0] = 0;
      }
    }
  }
  else if (systemState == 4) {//check state (partial)
    if (signalState[2] == 0 && (stepCountM1 < openStepTargetM1)) { //check open disable signal
      if (highSignalValidationCounter[0] > 0) {//check open command signal
        systemState = 0; //set open motion change status to oppening
        lastTd = 0.0;
        highSignalValidationCounter[0] = 0;
      }
    }
    if (signalState[3] == 0 && (stepCountM1 > closeStepTargetM1)) { //check close disable signal
      if (highSignalValidationCounter[1] > 0) { //check close command signal
        systemState = 1;//set close motion change status to closing
        highSignalValidationCounter[1] = 0;
      }
    }
  }
}

//open move control
void openStepControlM1() {
  digitalWrite(dirPinM1, 1);//sets movement direction
  speedControlM1();
  if (currentMicros - stepMicrosM1 >= stepDelayM1) { // move open in constant speed
    nextStepM1 = !nextStepM1;//swap next step state for M1
    digitalWrite(stepPinM1, nextStepM1);// moves motor
    nextStepM1 = !nextStepM1;//swap next step state for M1
    digitalWrite(stepPinM1, nextStepM1);// moves motor
    stepCountM1++;
    stepMicrosM1 = currentMicros;
  }
}

//close move control
void closeStepControlM1() {
  speedControlM1();
  digitalWrite(dirPinM1, 0);//sets movement direction
  if (currentMicros - stepMicrosM1 >= stepDelayM1) { // move close in constant speed
    nextStepM1 = !nextStepM1;//swap next step state for M1
    digitalWrite(stepPinM1, nextStepM1);// moves motor
    nextStepM1 = !nextStepM1;//swap next step state for M1
    digitalWrite(stepPinM1, nextStepM1);// moves motor
    stepCountM1--;//increment the step counter for M1
    stepMicrosM1 = currentMicros;
  }
}

/////////////////////////////////////////////////////////////

//Acceleration

// x = (t - t0) / (t1 - t0)   <=>   t = (1-x)*t0 + x*t1;  multiply by n = (t1 - t0)/r to get the actual step count


double  steps0(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x) * x;
}

double  steps1(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * (2 - x)) * x;
}

double  steps2(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * (5 - x*(6 - x*2))) * x;
}

double  steps3(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * x * (14 - x*(28 - x*(20 - x*5)))) * x;
}

double  steps4(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * x * x * (42 - x*(120 - x*(135 - x*(70 - x*14))))) * x;
}

// All curves have the same number of steps for the same (t0,t1,v0,v1,r) parameter set, if we ignore rounding errors.
double  steps(const double t0, const double t1, const double v0, const double v1, const double r) {
    return (t1 - t0) * (v1 + v0) / (r + r);
}

// Suffices for microsecond precision
#define  EPSILON  0.00000024

double  find_step0_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step0_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps0(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step1_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step1_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps1(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step2_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step2_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps2(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step3_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step3_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps3(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step4_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step4_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps4(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

//delays
double accDelay(int step) {
  f =  find_step4_time (stepCountM1,0, 0, Vmax, rampTime, stepLenghtM1);
  d = abs(f - lastTd);
  lastTd = f;
  return d;
}

//(10.801 20.801 250 10 0.225)
/////////////////////////////////////////////////////////////
/*
double  c0;
double  s0(double t) {
  return t * t * c0;
}

double  c10, c11;
double  s1(double t) {
  return t * t * t * (c10 + t * c11);
}

double  c20, c21, c22;
double  s2(double t) {
  return t * t * t * t * (c20 + t * (c21 + t * c22));
}

double  c30, c31, c32, c33;
double  s3(double t) {
  return t * t * t * t * t * (c30 + t * (c31 + t * (c32 + t * c33)));
}

// Output is in microseconds precision
const double  EPS = 0.00000024;

// Returns t for s(t) = v, if tmin <= t <= tmax, with maximum error teps in t.
double  find_t(double (*s)(double t), const double v, double tmin, double tmax, const double teps)
{
  // Low bound -- unnecessary check
  if (s(tmin) >= v)
    return tmin;

  // Upper bound.  Because of rounding errors, this may occur at the final step.
  if (s(tmax) <= v)
    return tmax;

  // Binary search.
  while (1) {
    double  t = 0.5 * (tmin + tmax);

    // Sufficient precision reached?  The equality checks catch the case where teps is so small it is within rounding error in calculating s(t).
    if (t == tmin || t == tmax || tmax - tmin <= teps) {
      return t;
    }

    double sv = s(t);
    if (sv < v) {
      // t is not in the lower half
      tmin = t;
    } else if (sv > v) {
      // t is not in the upper half
      tmax = t;
    } else {
      // sv == v
      return t;
    }
  }
}

void prepare_s0(const double R, const double T, const double Vmax)
{
  c0 = 0.5 * Vmax / (R * T);
}

void prepare_s1(const double R, const double T, const double Vmax)
{
  const double  T2 = T * T;
  c10 = Vmax / (R * T2);
  c11 = -0.5 * Vmax / (R * T2 * T);
}

void prepare_s2(const double R, const double T, const double Vmax)
{
  const double  T2 = T * T;
  const double  T4 = T2 * T2;
  c20 = 2.5 * Vmax / (R * T2 * T);
  c21 = -3.0 * Vmax / (R * T4);
  c22 = Vmax / (R * T4 * T);
}

void prepare_s3(const double R, const double T, const double Vmax)
{
  const double  T2 = T * T;
  const double  T4 = T2 * T2;
  c30 = 7.0 * Vmax / (R * T4);
  c31 = -14.0 * Vmax / (R * T4 * T);
  c32 = 10.0 * Vmax / (R * T4 * T2);
  c33 = -2.5 * Vmax / (R * T4 * T2 * T);
}

//delays
double accDelay(int step) {
  f = find_t(s3, step, 0, T, EPS);
  d = abs(f - lastTd);
  lastTd = f;
  return d;
}
*/

//speed control
void speedControlM1() {
  if (stepMicrosM1 <= (rampTime*1000000)) { //selects acceleration ramp portion of distance
    stepDelayM1 = accDelay(stepCountM1) * 1000000;
  } //accelerates up to max speed
  else if (stepMicrosM1 > (rampTime*1000000) && stepMicrosM1 <= ((time*1000000) - (rampTime*1000000))) { //selects max speed portion of time
    stepDelayM1 =  (time * 1000000) / stepTargetM1; // keeps max speed flat
  }
  else if (stepMicrosM1 > ((time*1000000) - (rampTime*1000000))) { //selects deceleration ramp portion of distance
    stepDelayM1 = accDelay(openStepTargetM1 - stepCountM1) * 1000000;
  }
  linearSpeedM1 = R/d;
  
}

void failSafe() {

}

void setup() {
  Serial.begin(115200);

  //global system pins modes
  pinMode (openPin, INPUT);
  pinMode (closePin, INPUT);
  pinMode (openDisablePin, INPUT);
  pinMode (closeDisablePin, INPUT);
  pinMode (ledPin, OUTPUT);

  //M1 pin modes
  pinMode(stepPinM1, OUTPUT);
  pinMode(dirPinM1, OUTPUT);
  pinMode(openLimitPinM1, INPUT);
  pinMode(closeLimitPinM1, INPUT);
  pinMode(adjustOpenLimitPinM1, INPUT);
  pinMode(adjustCloseLimitPinM1, INPUT);

  //M2 pin modes
  pinMode(stepPinM2, OUTPUT);
  pinMode(dirPinM2, OUTPUT);
  pinMode(openLimitPinM2, INPUT);
  pinMode(closeLimitPinM2, INPUT);
  pinMode(adjustOpenLimitPinM2, INPUT);
  pinMode(adjustCloseLimitPinM2, INPUT);

  //system variables initial set value
  lastDebounceTime = 0;
  lastDebugTime = 0;
  debounceDelay = 10;
  systemState = 3;

  //global system variables initial calculated value
  distancePerRev = numberTeethPulley * pulleyPace;// milimeters (18*5) = 90
  rampTime = (time * (accelerationRamp / 100)); // seconds 5 * (25/100) = 1.25

  //M1 variables initial value
  revolutionTargetM1 = (distanceM1 / (numberTeethPulley * pulleyPace));// revolutions (2800/ (18*5)) = 31.1111
  stepTargetM1 = (revolutionTargetM1 * stepPerRevolution); // total steps 31.1111*400 = 12444.4444
  maxLinearSpeedM1 = distanceM1 / time; // Vmax (milimeters per second) 2800/5 = 560
  angularSpeedM1 = revolutionTargetM1 / time; // steps per second 12444.444/5 = 2488.8888
  RPMM1 = ((revolutionTargetM1 / time) * 60); // revolutions per minute to complete set distace in set time (31.1111/5)*60 = 373.3333
  stepDelayM1 = (time * 1000000) / stepTargetM1; // seconds per step 5/12444.444 = 0.000401785 ..... * 1000000 = 401.7857 micros per step
  stepLenghtM1 = distancePerRev / stepPerRevolution; // R (mm per step)

  /*rampTime = (time * (accelerationRamp / 100)); // seconds 5 * (25/100) = 1.25
    angularAccelerationM1 = angularSpeedM1 / rampTime; //steps per second per second 2488.888/(5*0.25) = 1991.1104
    linearAccelerationM1 = maxLinearSpeedM1 / (rampTime); //milimeters per second per second 560/(5*0.25) = 448
    accelerationRampDistanceM1 = (linearAccelerationM1 * (accelerationRamp * accelerationRamp)) / 2; //milimeters (448*((5*0.25)^2))/2 = 350
    accelerationRampRevolutionsM1 = accelerationRampDistanceM1 / distancePerRev; //revolutions (350/90) = 3.8888
    accelerationRampStepsM1 = accelerationRampRevolutionsM1 * stepPerRevolution; // steps 3.8888 * 400 = 1555.5555
    stepDelayM1 = (time * 1000000) / stepTargetM1; // seconds per step 5/12444.444 = 0.000401785 ..... * 1000000 = 401.7857 micros per step
    stepDelayAccelerationM1 = stepDelayM1 / accelerationRampStepsM1; // micros per step per step 401.7857/1555.5555 = 0.258290
  */

  accelerationRampDistanceM1 = distanceM1 * (accelerationRamp / 100); // L = acceleration distance
  //stepDelayAccelerationM1 = ARRAY; // micros per step per step
  stepCountM1 = 0;
  closeStepTargetM1 = 0;
  openStepTargetM1 = stepTargetM1;

  //M2 variables initial calculated value
  revolutionTargetM2 = (distanceM2 / (numberTeethPulley * pulleyPace));// revolutions (2800/ (18*5)) = 31.1111
  stepTargetM2 = (revolutionTargetM2 * stepPerRevolution); // total steps 31.1111*400 = 12444.4444
  linearSpeedM2 = distanceM2 / time; // milimeters per second 2800/5 = 560
  angularSpeedM2 = revolutionTargetM2 / time; // steps per second 12444.444/5 = 2488.8888
  RPMM2 = ((revolutionTargetM2 / time) * 60); // revolutions per minute to complete set distace in set time (31.1111/5)*60 = 373.3333
  rampTime = (time * (accelerationRamp / 100)); // seconds 5 * (25/100) = 1.25
  angularAccelerationM2 = angularSpeedM2 / rampTime; //steps per second per second 2488.888/(5*0.25) = 1991.1104
  linearAccelerationM2 = linearSpeedM2 / (rampTime); //milimeters per second per second 560/(5*0.25) = 448
  accelerationRampDistanceM2 = (linearAccelerationM2 * (accelerationRamp * accelerationRamp)) / 2; //milimeters (448*((5*0.25)^2))/2 = 350
  accelerationRampRevolutionsM2 = accelerationRampDistanceM2 / distancePerRev; //revolutions (350/90) = 3.8888
  accelerationRampStepsM2 = accelerationRampRevolutionsM2 * stepPerRevolution; // steps 3.8888 * 400 = 1555.5555
  stepDelayM2 = time / stepTargetM2; // seconds per step 5/12444.444 = 0.000401785 ..... * 1000000 = 401.7857 micros per step
  stepDelayAccelerationM2 = stepDelayM2 / accelerationRampStepsM2; // micros per step per step 401.7857/1555.5555 = 0.258290
  stepCountM2 = 0;
  closeStepTargetM2 = 0;
  openStepTargetM2 = stepTargetM2;

  //arrays initial set values
  for (int i = 0; i < numSignals ; i++) {
    signalValidation[i] = 0; //validation output (1=valid)
    signalState[i] = 0;      // current state of the button
    lastSignalState[i] = 0;  // previous state of the button
  }
  for (int i = 0; i < 2 ; i++) {
    highSignalValidationCounter[i] = 0; // signal counter for valid open or close signals
    lowSignalValidationCounter[i] = 0; // signal counter for valid open or close signals
  }

  //acceleration initial settings
  L = accelerationRampDistanceM1;
  R = stepLenghtM1;
  Vmax = maxLinearSpeedM1;
  N = (long)(0.5 + (L / R)); // N = Round number of steps halfway upwards
  T = (double) ((2 * N * R) / Vmax); // Total duration is the same in all cases
  lastTd = 0.0;
  /*prepare_s0(R, T, Vmax);// Precalculate coefficients for fast s() function evaluation
  prepare_s1(R, T, Vmax);// Precalculate coefficients for fast s() function evaluation
  prepare_s2(R, T, Vmax);// Precalculate coefficients for fast s() function evaluation
  prepare_s3(R, T, Vmax);// Precalculate coefficients for fast s() function evaluation
*/
}

void loop() {

  currentMicros = micros(); //get the current micros at the beginning of the loop

  currentMillis = millis(); //get the current millis at the beginning of the loop

  signalVal (); //validates input signals

  stateEval (); //evaluates and commands motion state

  debug(1, 10);

  //openMoveAdjustment ();//adjusts open movement target

  //closeMoveAdjustment ();//adjusts close movement target

  //failSafe();//sets fail safes

}

String dToStr (double val, byte precision=7){
  // prints val with number of decimal places determine by precision
  // precision is a number from 0 to 6 indicating the desired decimial places
  // example: printDouble( 3.1415, 2); // prints 3.14 (two decimal places)

  String s = String(int(val));  //prints the int part
  if(precision > 0) {
    s += "."; // print the decimal point
    unsigned long frac;
    unsigned long mult = 1;
    byte padding = precision -1;
    while(precision--)
       mult *=10;
       
    if(val >= 0)
      frac = (val - int(val)) * mult;
    else
      frac = (int(val) - val) * mult;
    unsigned long frac1 = frac;
    while(frac1 /= 10)
      padding--;
    while(padding--)
      s+= "0";
    s += String(frac,DEC);
  }
  return s;
}


void debug(int active, int period) {
  if (active && (currentMillis - lastDebugTime > period)) {

    /*String state;
      if (systemState == 0) state = "Opening";
      else if (systemState == 1) state = "Closing";
      else if (systemState == 2) state = "Openned";
      else if (systemState == 3) state = "Closed";
      else if (systemState == 4) state = "Partial";
      String s = "systemState: " + state + "("+String (systemState)+")" + ", stepCountM1: " + String (stepCountM1);
    */

    //String s = "openStepTargetM1: " + String(openStepTargetM1) + ", closeStepTargetM1: " + String(closeStepTargetM1);

    //String s = "N: " + String(N) + ", L: " + String(L) + ", R: " + String(R) + ", T: " + String(T) + ", Vmax: " + String(Vmax);

    //String s = "stepCountM1: " + String(stepCountM1) + " stepDelayM1: " + String (stepDelayM1);

    //String s = "stepCountM1: " + String(stepCountM1) + " linearSpeedM1: " + String (linearSpeedM1);

    //String s = String(stepCountM1) + "  " + String (linearSpeedM1);

    String s = "d: " + dToStr(d) + " f: " + dToStr(f);

    lastDebugTime = currentMillis;

    Serial.println(s);
  }
}


//////////////////////////////////////////////////////////////////


/*open move adjustment
  void openMoveAdjustment () {
  while (digitalRead (openLimitPinM1) == 1) {
  moveOpenM1 = 0;
  openStepTargetM1 = stepCountM1;
  }
  while (digitalRead(adjustOpenLimitPinM1) == 1) {
  if (adjustOpeNM1 == openStepTargetM1) {
  openStepTargetM1 *= (1 + (openAdjustmentM1 / 100));
  moveOpenM1 = 1;
  moveCloseM1 = 0;
  }
  }
  }

  // close move adjustment
  void closeMoveAdjustment () {
  while (digitalRead (closeLimitPinM1) == 1) {
  moveCloseM1 = 0;
  closeStepTargetM1 = stepCountM1;
  }
  while (digitalRead(adjustCloseLimitPinM1) == 1) {
  if (adjustCloseTempM1 == closeStepTargetM1) {
  closeStepTargetM1 *=  (1 - (closeAdjustmentM1 / 100));
  moveOpenM1 = 0;
  moveCloseM1 = 1;
  }
  }
  }
*/

///////////////////////////////////////////
/*

NEW ACCELERATION CODE
  // SPDX-License-Identifier: CC0-1.0
#include <stdint.h>

// x = (t - t0) / (t1 - t0)   <=>   t = (1-x)*t0 + x*t1;  multiply by n = (t1 - t0)/r to get the actual step count
double  steps0(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x) * x;
}

double  steps1(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * (2 - x)) * x;
}

double  steps2(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * (5 - x*(6 - x*2))) * x;
}

double  steps3(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * x * (14 - x*(28 - x*(20 - x*5)))) * x;
}

double  steps4(const double x, const double v0, const double v1) {
    return (v0 + 0.5 * (v1 - v0) * x * x * x * x * x * (42 - x*(120 - x*(135 - x*(70 - x*14))))) * x;
}

// All curves have the same number of steps for the same (t0,t1,v0,v1,r) parameter set, if we ignore rounding errors.
double  steps(const double t0, const double t1, const double v0, const double v1, const double r) {
    return (t1 - t0) * (v1 + v0) / (r + r);
}

// Suffices for microsecond precision
#define  EPSILON  0.00000024

double  find_step0_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step0_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps0(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step1_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step1_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps1(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step2_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step2_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps2(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step3_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step3_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps3(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

double  find_step4_time(const uint32_t n, double t0, double t1, const double v0, const double v1, const double r) {
    if (t0 > t1)
        return find_step4_time(n, t1, t0, v1, v0, r);

    const double  target_s = (double)n * r / (t1 - t0);
    double        x0 = 0.0;
    double        x1 = 1.0;
    double        xeps = EPSILON * r / (t1 - t0);

    while (1) {
        const double  x = 0.5*(x0 + x1);
        if (x1 - x0 <= xeps)
            return (1-x)*t0 + x*t1;

        const double  s = steps4(x, v0, v1);
        if (s < target_s)
            x0 = x;
        else
        if (s > target_s)
            x1 = x;
        else
            return (1-x)*t0 + x*t1;
    }
}

// The rest is for the example Linux program below

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>

int parse_double(const char *src, double *to)
{
    if (!src || !*src)
        return -1;

    const char *end = src;
    double      val;

    errno = 0;
    val = strtod(src, (char **)&end);
    if (errno || !end || end == src)
        return -1;

    end += strspn(end, "\t\n\v\f\r ");
    if (*end)
        return -1;

    if (to)
        *to = val;
    return 0;
}

void usage(const char *arg0) {
    printf("Usage: %s [ -h || --help ]\n", arg0);
    printf("       %s t0 t1 v0 v1 r\n", arg0);
    printf("This calculates the times for (t1-t0)*(v0+v1)/(2*r) steps,\n");
    printf("starting at time t0 with velocity t0, and ending at time t1 with velocity v1.\n");
}

int main(int argc, char *argv[]) {
    const char *arg0 = (argc > 0 && argv && argv[0] && argv[0][0]) ? argv[0] : "(this)";

    double    t0, t1, v0, v1, r;

    if (argc < 2 || (argc >= 2 && (!strcmp(argv[1], "-h") || !strcmp(argv[1], "--help")))) {
        usage(arg0);
        return EXIT_SUCCESS;
    } else
    if (argc != 6) {
        usage(arg0);
        return EXIT_FAILURE;
    }

    if (parse_double(argv[1], &t0)) {
        fprintf(stderr, "%s: Invalid start time.\n", argv[1]);
        return EXIT_FAILURE;
    }

    if (parse_double(argv[2], &t1)) {
        fprintf(stderr, "%s: Invalid end time.\n", argv[2]);
        return EXIT_FAILURE;
    } else
    if (t0 >= t1) {
        fprintf(stderr, "%s: End time must be later than start time (%s).\n", argv[2], argv[2]);
        return EXIT_FAILURE;
    }

    if (parse_double(argv[3], &v0)) {
        fprintf(stderr, "%s: Invalid initial velocity.\n", argv[3]);
        return EXIT_FAILURE;
    } else
    if (v0 <= 0.0) {
        fprintf(stderr, "%s: Initial velocity must be positive and nonzero.\n", argv[3]);
        return EXIT_FAILURE;
    }

    if (parse_double(argv[4], &v1)) {
        fprintf(stderr, "%s: Invalid final velocity.\n", argv[4]);
        return EXIT_FAILURE;
    } else
    if (v1 <= 0.0 || v1 == v0) {
        fprintf(stderr, "%s: Final velocity must be positive, nonzero, and different to initial velocity (%s).\n", argv[4], argv[3]);
        return EXIT_FAILURE;
    }

    if (parse_double(argv[5], &r)) {
        fprintf(stderr, "%s: Invalid step length.\n", argv[5]);
        return EXIT_FAILURE;
    } else
    if (r <= 0.0) {
        fprintf(stderr, "%s: Step length must be positive and nonzero.\n", argv[5]);
        return EXIT_FAILURE;
    }

    uint32_t i = 0;
    uint32_t n = (uint32_t)steps(t0, t1, v0, v1, r);

    if (n < 3) {
        fprintf(stderr, "Not enough steps.\n");
        return EXIT_FAILURE;
    }

    double  next0 = t0, next1 = t0, next2 = t0, next3 = t0, next4 = t0;

    printf("#step           time_0       time_1       time_2       time_3       time_4      interval_0   interval_1   interval_2   interval_3   interval_4\n");
    while (1) {

        printf("%-9lu %12.6f %12.6f %12.6f %12.6f %12.6f", (unsigned long)i, next0, next1, next2, next3, next4);
        if (++i > n) {
            printf("\n");
            break;
        }
        const double prev0 = next0, prev1 = next1, prev2 = next2, prev3 = next3, prev4 = next4;

        next0 = find_step0_time(i, t0, t1, v0, v1, r);
        next1 = find_step1_time(i, t0, t1, v0, v1, r);
        next2 = find_step2_time(i, t0, t1, v0, v1, r);
        next3 = find_step3_time(i, t0, t1, v0, v1, r);
        next4 = find_step4_time(i, t0, t1, v0, v1, r);

        printf("   %12.6f %12.6f %12.6f %12.6f %12.6f\n", next0 - prev0, next1 - prev1, next2 - prev2, next3 - prev3, next4 - prev4);
    }

    return EXIT_SUCCESS;
}
*/