#include <Arduino.h>
#include "BasicStepperDriver.h"

#define SYNC_ON            2

#define X_STEP_PIN         8
#define X_DIR_PIN          7
#define X_ENABLE_PIN       12

#define FC      A2
#define POTA    A1
#define POTB    A3
#define MODE    6
#define MASTER  3
#define FIN_IN  A7
#define FIN_OUT 4
#define INI_IN  5
#define INI_OUT A5

int MS[3] = {9, 10, 11};

unsigned long POT_VALUE_A = 0;
unsigned long POT_VALUE_B = 0;


// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200
#define RPM 300

// Since microstepping is set externally, make sure this matches the selected mode
// If it doesn't, the motor will move at a different RPM than chosen
// 1=full step, 2=half step etc.
#define MICROSTEPS 32

//Uncomment line to use enable/disable functionality
BasicStepperDriver stepperX(MOTOR_STEPS, X_DIR_PIN, X_STEP_PIN, X_ENABLE_PIN );

#define N 1
uint32_t average(int PIN)
  {
    uint32_t value = 0;
    for(int i=0; i<N; i++)
      {
        value = analogRead(PIN) + value;
      }
    return value/N;
  }

void setup() {
  Serial.begin(115200);
  for(int i=0; i<3; i++)
    {
      pinMode(MS[i], OUTPUT); 
      digitalWrite(MS[i], HIGH);
    }
  pinMode(FIN_OUT, OUTPUT); 
  pinMode(INI_OUT, OUTPUT); 
  digitalWrite(FIN_OUT, HIGH); 
  digitalWrite(INI_OUT, HIGH); 
  pinMode(FC, INPUT_PULLUP); 
  pinMode(MODE, INPUT_PULLUP); 
  pinMode(INI_IN, INPUT_PULLUP); 
  pinMode(MASTER, INPUT_PULLUP); 
  stepperX.begin(RPM, MICROSTEPS);
  stepperX.setEnableActiveState(LOW);

  stepperX.enable();
  //reset_steppers(10000); 
}

void loop() {
    #if (SYNC_ON == 1)
      while ((analogRead(FIN_IN)<800)||(!digitalRead(INI_IN)));
      digitalWrite(INI_OUT, HIGH);
      delay(10);
      digitalWrite(INI_OUT, LOW);
      digitalWrite(FIN_OUT, LOW);
      if (digitalRead(MODE)) move_stepper(POTB);
      else  move_stepper(POTA);
      while (analogRead(FIN_IN)<800);
      digitalWrite(FIN_OUT, HIGH);
      if (!digitalRead(MASTER)) delay(1000);
    #else 
      #if (SYNC_ON == 2)
        if (digitalRead(MODE)) while ((analogRead(FIN_IN)<800)||(!digitalRead(INI_IN)));
        digitalWrite(INI_OUT, HIGH);
        delay(10);
        digitalWrite(INI_OUT, LOW);
        digitalWrite(FIN_OUT, LOW);
        if (digitalRead(MODE)) move_stepper(POTB);
        else  move_stepper(POTA);
        if (digitalRead(MODE)) while (analogRead(FIN_IN)<800);
        digitalWrite(FIN_OUT, HIGH);
        if ((!digitalRead(MASTER))||(!digitalRead(MODE))) delay(1000);
      #else 
        if (digitalRead(MODE)) move_stepper(POTB);
        else  move_stepper(POTA);
        delay(1000);
      #endif
    #endif
}

unsigned long time_escape = micros();

void reset_steppers(unsigned long TIME)
  {
    // energize coils - the motor will hold position
    stepperX.enable();
    time_escape = micros();
    while ((digitalRead(FC))&&((micros()-time_escape)<TIME))
      {
         if (digitalRead(FC)) stepperX.move(1);
         else stepperX.disable();
      }
  }


int pas = 1 * MICROSTEPS;
int MAX = pas * MOTOR_STEPS;
int freno = 100;
unsigned long  time = 0;
unsigned long  time_freno = 0;
int pot_value = 1023;
int sig = 1;

void move_stepper(int POT)
  { 
    stepperX.enable();
    pot_value = average(POT);
    if (digitalRead(FC)) sig=1;
    else sig=-1;
    
    if (pot_value<1010)
      {
        for(int i=0; i<MAX; i = i + pas)
          {   
             time = map(pot_value, 0, 1023, 0, 50);
             //stepperX.rotate(pas);
             stepperX.move(sig*pas);
             if (i<(MAX-freno)) 
              {
                delay(time);
                time_freno = time;
              }
             else 
              {
                delay(time_freno);
                time_freno = time_freno + (2*time)/freno;
              }
          }
      } 
    
    //stepperX.disable();
  }