#include <Wire.h>
#include <AccelStepper.h>
//-----------------------------------------------------------------------------
#define dirPin1 6
#define stepPin1 7
#define motorInterfaceType1 1
AccelStepper stepper = AccelStepper(motorInterfaceType1, stepPin1, dirPin1);
//variabili -------------------------------------------------------------------
//Pulsanti INVERSIONE ---------------------------------------------------------
int varsw1 = HIGH; // HIGH - LOW inverte lettura asse 0 scarico (2)
//-------------------
int varsw2 = HIGH; // HIGH - LOW inverte lettura asse 0 scarico (3)
int vartub = HIGH; // HIGH - LOW inverte lettura tubo scarico (3)
//-------------------
int ResPuls = LOW; // HIGH - LOW reset pulsante
int reseton = 1;    // 0 - 1 Reset Pulsante on - off
//-----------------------------------------------------------------------------
//reg. giri enc.- motore
int varpot = 1200;          // 1200 impulsi = 1 giro encoder
int varstep = 750;          //  400 impulsi = 1 giro motore
//reg. giri enc.- motore
//-----------------------------------------------------------------------------
//Scarico (3)
int maxspeed = 10000; //velocita di movimento(0 - 10000)
int acceleration = 10000; //velocita accelerazione iniziale(0 - 10000)
//-------------------
int maxspeedres = 500; //velocita di movimento nel reset(0 - 10000)
int accelerationres = 500; //velocita accelerazione iniziale nel reset(0 - 10000)
//Scarico (3)
//-----------------------------------------------------------------------------
//Seriale
int ENCSerial = 0;        //set 0 disable - 1 enable (Abilitazione Seriale)
int Serialab = 0;         //set 0 disable - 1 enable
//Seriale
//Variabili -------------------------------------------------------------------

//pin uscite digitali
#define encoderPinA  2
#define encoderPinB  3
//-------------------
#define resswitch1 4
//-------------------
#define resswitch2 5
#define tub 8
//-------------------
#define ResetPuls 13
// ------------------
volatile long pos = 0;
int pulsres = 0;
int reset = 0;
int res1 = 0;
int res2 = 0;
int pos1;
int tubeon = 0;
int dueres = 0;
void setup() {
  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(encoderPinB, INPUT_PULLUP);
  pinMode(resswitch1, INPUT_PULLUP);
  pinMode(resswitch2, INPUT_PULLUP);
  pinMode(ResetPuls, INPUT_PULLUP);
  pinMode(tub, INPUT_PULLUP);
  stepper.setMaxSpeed(maxspeed);
  stepper.setAcceleration(acceleration);
  attachInterrupt(0, doEncoderA, CHANGE);
  attachInterrupt(1, doEncoderB, CHANGE);
  if (Serialab == 1) {
    Serial.begin(9600);
  }
}

void loop() {
  //Velocità ---------------------------------------------------------------------------------------------------------------------
  if (reset == 1) { //velocità di funzionamento
    stepper.setMaxSpeed(maxspeed);
    stepper.setAcceleration(acceleration);
  }
  else { // velocità di reset
    stepper.setMaxSpeed(maxspeedres);
    stepper.setAcceleration(accelerationres);
  }
  // lettuta encoder posizione ---------------------------------------------------------------------------------------------------
  if (reset == 1) {
    pos1 = map(pos, 0, varpot, 0, varstep);
    //funzionamento DRIVER 1 -------------------------------------------------------------------------------------------------------
    if (digitalRead(tub) == vartub) { //lettura tubo
      tubeon = 1;
    }
    else if (digitalRead(tub) != vartub && digitalRead(resswitch1) != varsw1 && digitalRead(resswitch2) != varsw2 && tubeon == 1) { //reset scarico (3)
      tubeon = 0;
      delay(1000);
      pos = 0;
    }
    if (tubeon == 1) {
      if (digitalRead(resswitch2) != varsw2 && stepper.currentPosition() >= 1000) {
        reset = 2;
        stepper.runToNewPosition(0);
        tubeon = 0;
      }
      else {
        stepper.runToNewPosition(pos1);
      }
    }
  }
  //reset DRIVER 1 ---------------------------------------------------------------------------------------------------------------
  if (reset == 0) {
    if (pulsres == 0) {
      if (digitalRead(resswitch1) != varsw1 && reset == 0 && res1 == 1) { //controllo reset completato scarico (2) + avvio reset scarico (3)
        if (digitalRead(resswitch2) == varsw2) {
          stepper.moveTo(-100000);
          stepper.run();
          res2 = 1;
          //Serial.println("3");
        }
        else if (digitalRead(resswitch2) != varsw2 && res2 == 1) { //controllo reset del punto zero del encoder e del motore
          stepper.stop();
          stepper.setCurrentPosition(0);
          pos = 0;
          reset = 1;
          //Serial.println("5");
        }
        else { //reset scarico (3)
          stepper.runToNewPosition(2000);
          //Serial.println("4");
        }
      }
      else if (digitalRead(resswitch1) == varsw1 && dueres == 3 && reset == 0 && res1 == 0) { //controllo reset (2) scarico
        res1 = 1;
        //Serial.println("2");
      }
      else if (digitalRead(resswitch1) != varsw1 && dueres == 2) { //doppio reset
        dueres = 3;
        //Serial.println("1-2");
        delay(500);
      }
      else if (digitalRead(resswitch1) == varsw1 && dueres == 1) { //doppio reset
        dueres = 2;
        //Serial.println("1-1");
        delay(500);
      }
      else if (digitalRead(resswitch1) != varsw1 && dueres == 0) { //doppio reset
        dueres = 1;
        //Serial.println("1");
        delay(500);
      }
    }
  }
  //reset DRIVER 1 RESET PULSANTE-------------------------------------------------------------------------------------------------
  if (reseton == 1) {
    if (digitalRead(ResetPuls) == ResPuls) {
      //Serial.println("1");
      pulsres = 1;
      reset = 0;
      res1 = 1;
      res2 = 0;
    }
    if (pulsres == 1) {
      if (reset == 0) {
        if (reset == 0 && res1 == 1) { //controllo reset completato scarico (2) + avvio reset scarico (3)
          if (digitalRead(resswitch2) == varsw2) {
            stepper.moveTo(-100000);
            stepper.run();
            res2 = 1;
          }
          else if (digitalRead(resswitch2) != varsw2 && res2 == 1) { //controllo reset del punto zero del encoder e del motore
            stepper.stop();
            stepper.setCurrentPosition(0);
            pos = 0;
            reset = 1;
          }
          else { //reset scarico (3)
            stepper.runToNewPosition(2000);
          }
        }
      }
    }
  }
}
//encoder ----------------------------------------------------------------------------------------------------------------------
void doEncoderA() {
  if (digitalRead(encoderPinA) == HIGH) {
    if (digitalRead(encoderPinB) == LOW) {
      pos = pos + 1;         // CW
    }
    else {
      pos = pos - 1;         // CCW
    }
  }
  else {
    if (digitalRead(encoderPinB) == HIGH) {
      pos = pos + 1;          // CW
    }
    else {
      pos = pos - 1;          // CCW
    }
  }
}
void doEncoderB() {
  if (digitalRead(encoderPinB) == HIGH) {
    if (digitalRead(encoderPinA) == HIGH) {
      pos = pos + 1;         // CW
    }
    else {
      pos = pos - 1;         // CCW
    }
  }
  else {
    if (digitalRead(encoderPinA) == LOW) {
      pos = pos + 1;          // CW
    }
    else {
      pos = pos - 1;          // CCW
    }
  }
}