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

#define X_STEP_PIN         54
#define X_DIR_PIN          55
#define X_ENABLE_PIN       38

#define Y_STEP_PIN         60
#define Y_DIR_PIN          61
#define Y_ENABLE_PIN       56

#define Z_STEP_PIN         46
#define Z_DIR_PIN          48
#define Z_ENABLE_PIN       62

#define E_STEP_PIN         26
#define E_DIR_PIN          28
#define E_ENABLE_PIN       24

#define Q_STEP_PIN         36
#define Q_DIR_PIN          34
#define Q_ENABLE_PIN       30

int FC[6] = { 3, 2, 14, 15, 18, 19};
int POT[5] = { A3, A4, A5, A10, A12};
unsigned long POT_VALUE[5] = { 0, 0, 0, 0, 0};

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

// 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 );
BasicStepperDriver stepperY(MOTOR_STEPS, Y_DIR_PIN, Y_STEP_PIN, Y_ENABLE_PIN );
BasicStepperDriver stepperZ(MOTOR_STEPS, Z_DIR_PIN, Z_STEP_PIN, Z_ENABLE_PIN );
BasicStepperDriver stepperE(MOTOR_STEPS, E_DIR_PIN, E_STEP_PIN, E_ENABLE_PIN );
BasicStepperDriver stepperQ(MOTOR_STEPS, Q_DIR_PIN, Q_STEP_PIN, Q_ENABLE_PIN );

#define N 100
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<6; i++)
    {
      pinMode(FC[i], INPUT_PULLUP); 
    }
  stepperX.begin(RPM, MICROSTEPS);
  stepperY.begin(RPM, MICROSTEPS);
  stepperZ.begin(RPM, MICROSTEPS);
  stepperE.begin(RPM, MICROSTEPS);
  stepperQ.begin(RPM, MICROSTEPS);
  // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line
  stepperX.setEnableActiveState(LOW);
  stepperY.setEnableActiveState(LOW);
  stepperZ.setEnableActiveState(LOW);
  stepperE.setEnableActiveState(LOW);
  stepperQ.setEnableActiveState(LOW);

  stepperX.enable();
  stepperY.enable();
  stepperZ.enable();
  stepperE.enable();
  stepperQ.enable();
  //reset_steppers(10000); 
}

void loop() {
  if (digitalRead(FC[5]))
    {
      POT_VALUE[0]=map(average(POT[0]), 0, 1023, 0, 2000);
      move_steppers(POT_VALUE[0], POT_VALUE[0], POT_VALUE[0], POT_VALUE[0], POT_VALUE[0]);
      delay(1000);
    }
  else
    {
       for(int i=0; i<5; i++) 
        {
          POT_VALUE[i]=map(average(POT[i]), 0, 1023, 0, 20000);
        }
       //Serial.println();
       move_steppers(POT_VALUE[0], POT_VALUE[1], POT_VALUE[2], POT_VALUE[3], POT_VALUE[4]);
       delay(1000);
    }

  
//  if (digitalRead(FC[5]))
//    {
//      move_steppers(0, 0, 0, 0, 0, 1000);
//      move_steppers(350, 45, 45, 45, 45, 1000);
//      move_steppers(10, 90, 60, 30, 15, 1000);
//    }
//  else
//    {
//       for(int i=0; i<5; i++) 
//        {
//          POT_VALUE[i]=map(average(POT[i]), 0, 1023, 0, 359);
//          //Serial.print(POT_VALUE[i]);
//          //Serial.print(", ");
//        }
//       //Serial.println();
//       move_steppers(POT_VALUE[0], POT_VALUE[1], POT_VALUE[2], POT_VALUE[3], POT_VALUE[4], 100);
//    }
}

unsigned long time_escape = micros();

void reset_steppers(unsigned long TIME)
  {
    // energize coils - the motor will hold position
    stepperX.enable();
    stepperY.enable();
    stepperZ.enable();
    stepperE.enable();
    stepperQ.enable();
    time_escape = micros();
    while (((digitalRead(FC[0]))||(digitalRead(FC[1]))||(digitalRead(FC[2]))||(digitalRead(FC[3]))||(digitalRead(FC[4])))&&((micros()-time_escape)<TIME))
      {
         if (digitalRead(FC[0])) stepperX.move(1);
         else stepperX.disable();
         if (digitalRead(FC[1])) stepperY.move(1);
         else stepperY.disable();
         if (digitalRead(FC[2])) stepperZ.move(1);
         else stepperZ.disable();
         if (digitalRead(FC[3])) stepperE.move(1);
         else stepperE.disable();
         if (digitalRead(FC[4])) stepperQ.move(1);
         else stepperQ.disable();
      }
  }


int stp_actual[5] = { 0, 0, 0, 0, 0};
int stp_move[5] = { 0, 0, 0, 0, 0};
int stp[5] = { 0, 0, 0, 0, 0};
int dir[5] = { 1, 1, 1, 1, 1};

void move_steppers(int stp1, int stp2, int stp3, int stp4, int stp5, unsigned long TIME)
  {
     
    stp[0] = stp1;
    stp[1] = stp2;
    stp[2] = stp3;
    stp[3] = stp4;
    stp[4] = stp5;
    for(int i=0; i<5; i++)
     {
        if (stp[i]>359) stp[i] = 0;
        stp_move[i] =  stp[i] - stp_actual[i];
        if (stp_move[i]<0) 
          {
            dir[i] = -1;
            stp_move[i] = abs(stp_move[i]);
          }
        else dir[i] = 1;
        //Serial.print("a=");
        //Serial.print(stp_actual[i]);
        stp_actual[i] = stp[i];
        //Serial.print(", f=");
        //Serial.print(stp_actual[i]);
        //Serial.print(", m=");
        //Serial.print(stp_move[i]*dir[i]);
        //Serial.print(" ");
     }
    Serial.println();
    
    if (stp_move[0]!=0) stepperX.enable();
    if (stp_move[1]!=0) stepperY.enable();
    if (stp_move[2]!=0) stepperZ.enable();
    if (stp_move[3]!=0) stepperE.enable();
    if (stp_move[4]!=0) stepperQ.enable();
    
    int count = 0;
    while ((count<stp_move[0])||(count<stp_move[1])||(count<stp_move[2])||(count<stp_move[3])||(count<stp_move[4]))
      {
        if (count<stp_move[0]) stepperX.rotate(dir[0]);
        else stepperX.disable();
        if (count<stp_move[1]) stepperY.rotate(dir[1]);
        else stepperY.disable();
        if (count<stp_move[2]) stepperZ.rotate(dir[2]);
        else stepperZ.disable();
        if (count<stp_move[3]) stepperE.rotate(dir[3]);
        else stepperE.disable();
        if (count<stp_move[4]) stepperQ.rotate(dir[4]);
        else stepperQ.disable();
        count++;
      }
    delay(TIME);
  }

unsigned long time_pas1 = micros();
unsigned long time_pas2 = micros();
unsigned long time_pas3 = micros();
unsigned long time_pas4 = micros();
unsigned long time_pas5 = micros();
int pas = 10;
   
void move_steppers(unsigned long time1, unsigned long time2, unsigned long time3, unsigned long time4, unsigned long time5)
  {   
    unsigned long TIME[5] = { (time1*pas)/360, (time2*pas)/360,(time3*pas)/360,(time4*pas)/360,(time5*pas)/360};
    int count1 =0;
    int count2 =0;
    int count3 =0;
    int count4 =0;
    int count5 =0;
    stepperX.enable();
    stepperY.enable();
    stepperZ.enable();
    stepperE.enable();
    stepperQ.enable();
    while ((count1<360/pas)||(count2<360/pas)||(count3<360/pas)||(count4<360/pas)||(count5<360/pas))
      {
        while (count1<360/pas)
          {
            stepperX.rotate(pas);
            delay(TIME[0]);
            count1++;
          }
        //else stepperX.disable();
        while (count2<360/pas)
          {
            stepperY.rotate(pas);
            delay(TIME[1]);
            count2++;
          }
        //else stepperY.disable();
        while (count3<360/pas)
          {
            stepperZ.rotate(pas);
            delay(TIME[2]);
            count3++;
          }
        //else stepperZ.disable();
        while (count4<360/pas)
          {
            stepperE.rotate(pas);
            delay(TIME[3]);
            count4++;
          }
        //else stepperE.disable();
        while (count5<360/pas)
          {
            stepperQ.rotate(pas);
            delay(TIME[4]);
            count5++;
          }
        //else stepperQ.disable();
      } 
    count1 =0;
    count2 =0;
    count3 =0;
    count4 =0;
    count5 =0;
    stepperX.disable();
    stepperY.disable();
    stepperZ.disable();
    stepperE.disable();
    stepperQ.disable();
  }