class MillisAlarm {
  private:
    unsigned long interval;
    unsigned long startTime;

  public:
    void start(unsigned long timerLength) {
      interval = timerLength;
      startTime = millis();
    }

    bool done() {
      return (millis() - startTime >= interval);
    }

};

class MillisStopWatch {
  private:
    unsigned long startTime;

  public:
    void start() {
      startTime = millis();
    }

    unsigned long check() {
      return millis() - startTime;
    }
};

MillisAlarm alarm;
MillisStopWatch stopWatch;


#include <ezButton.h>
#define LOOP_STATE_STOPPED 0
#define LOOP_STATE_STARTED 1
#include <AccelStepper.h>
#define HALFSTEP 4
int stepRev = 200;
ezButton button(12);  // create ezButton object that attach to pin D12;
int loopState = LOOP_STATE_STOPPED;
// ========================SCREW
// Define stepper motor connections and motor interface type.
// Motor interface type must be set to 1 when using a driver:
#define dirPin 19
#define stepPin 18
#define motorInterfaceType 1
#define home_screw 2  // Pin D2 connected to Home Switch (MicroSwitch)


long initial_homing = 1;  // Used to Home Stepper at startup

// Create a new instance of the AccelStepper class:
AccelStepper ScrewStepper(motorInterfaceType, stepPin, dirPin);


long pos0 = -stepRev * 1.75 * (10 / 1.5);
long pos1 = -stepRev * 1.75 * (25 / 1.5);
long pos2 = -stepRev * 1.75 * (65 / 1.5);

// ========================GUIDE
// define number of step per revolution
int stepsPerRevolution = 2048;
// Create stepper istance

#define home_guide 1  // Pin D1 connected to Home Switch (MicroSwitch)

#define motorPin1 3  // IN1 on ULN2003 ==> Blue   on 28BYJ-48
#define motorPin2 4  // IN2 on ULN2004 ==> Pink   on 28BYJ-48
#define motorPin3 5  // IN3 on ULN2003 ==> Yellow on 28BYJ-48
#define motorPin4 6  // IN4 on ULN2003 ==> Orange on 28BYJ-48
AccelStepper GuideStepper(HALFSTEP, motorPin1, motorPin3, motorPin2, motorPin4);


long GuideHoming = stepsPerRevolution;
long StepPermm = stepsPerRevolution / (1.5 * 15 * 3.1415);
long posGuiON = -StepPermm * 5;
long posGuiOFF = -StepPermm * 35;


// ========================LEVER
// Create stepper istance

#define motorPin5 14  // IN1 on ULN2003 ==> Blue   on 28BYJ-48
#define motorPin6 15  // IN2 on ULN2004 ==> Pink   on 28BYJ-48
#define motorPin7 16  // IN3 on ULN2003 ==> Yellow on 28BYJ-48
#define motorPin8 17  // IN4 on ULN2003 ==> Orange on 28BYJ-48
AccelStepper LeverStepper(HALFSTEP, motorPin5, motorPin7, motorPin6, motorPin8);

long LeverHoming = stepsPerRevolution * 0.5;
long StepPerdeg = stepsPerRevolution / 360;
long LeverON = -StepPerdeg * 45;
long LeverOFF = StepPerdeg * 10;


void setup() {

  //start an alarm that will end in 1 second
  alarm.start(1000);

  //start a stopWatch, timer starting @ current millis();
  stopWatch.start();

  Serial.begin(9600);
  while (!Serial);

  button.setDebounceTime(100); // set debounce time to 50 milliseconds

  GuideStepper.setMaxSpeed(1000);
  GuideStepper.setAcceleration(100);
  GuideStepper.setSpeed(1000);

  LeverStepper.setMaxSpeed(1000);
  LeverStepper.setAcceleration(100);
  LeverStepper.setSpeed(1000);

  // // Homing screw
  // // Start Homing procedure of Stepper Motor at startup
  // pinMode(home_screw, INPUT_PULLUP);
  // ScrewStepper.setMaxSpeed(50);
  // ScrewStepper.setAcceleration(50);


  // while (digitalRead(home_screw)) {       // Make the Stepper move CW until the switch is activated
  //     ScrewStepper.moveTo(initial_homing);  // Set the position to move to
  //     initial_homing++;
  //     ScrewStepper.run();
  // }


  // ScrewStepper.setMaxSpeed(5000);       // Set Max Speed of Stepper (Slower to get better accuracy)
  // ScrewStepper.setAcceleration(500);    // Set Acceleration of Stepper
  // initial_homing = -1;

  // while (!digitalRead(home_screw)) {  // Make the Stepper move CCW until the switch is deactivated
  //     ScrewStepper.moveTo(initial_homing);
  //     initial_homing--;
  //     ScrewStepper.run();
  // }
  // ScrewStepper.setCurrentPosition(0);
  // ScrewStepper.moveTo(pos0);
  // ScrewStepper.runToPosition();

  Serial.println("Homing estrusore eseguito");


  // // Homing Lever
  // // Start Homing procedure of Stepper Motor at startup
  LeverStepper.moveTo(LeverHoming);
  LeverStepper.runToPosition();
  if (LeverStepper.distanceToGo() == 0) {
    LeverStepper.setCurrentPosition(0);
  }
  LeverStepper.moveTo(LeverON);
  LeverStepper.runToPosition();
  Serial.println("Homing leva eseguita");

  // // Homing Guide
  // // Start Homing procedure of Stepper Motor at startup
  GuideStepper.moveTo(GuideHoming);
  GuideStepper.runToPosition();
  GuideStepper.setCurrentPosition(0);
  GuideStepper.moveTo(posGuiON);
  GuideStepper.runToPosition();
  Serial.println("Homing guida eseguita");
}

void loop() {

  button.loop(); // MUST call the loop() function first

  if (button.isPressed()) {

    if (loopState == LOOP_STATE_STOPPED) {
      loopState = LOOP_STATE_STARTED;
      String struno = "Button pressed: ";
      String strbot = struno + loopState;
      Serial.println(strbot);

    }
    else //if (loopState == LOOP_STATE_STARTED) {
    { loopState = LOOP_STATE_STOPPED;
      String struno = "Button pressed: ";
      String strbot = struno + loopState;
      Serial.println(strbot);

    }
  }

  if (loopState == LOOP_STATE_STARTED) {
    Serial.println("Inizio ciclo");

    LeverStepper.moveTo(LeverHoming);
    LeverStepper.runToPosition();
    if (LeverStepper.distanceToGo() == 0) {
      LeverStepper.setCurrentPosition(0);
    }

    alarm.start(500);

    LeverStepper.moveTo(LeverON);
    LeverStepper.runToPosition();

    // // Set the maximum speed and acceleration in working:
    // ScrewStepper.setMaxSpeed(5000);
    // ScrewStepper.setAcceleration(500);

    // ScrewStepper.moveTo(pos1);
    // ScrewStepper.runToPosition();

    // ScrewStepper.setMaxSpeed(500);
    // ScrewStepper.setAcceleration(200);

    // ScrewStepper.moveTo(pos2);
    // ScrewStepper.runToPosition();
    // alarm.start(1500);



    GuideStepper.moveTo(posGuiOFF);
    GuideStepper.runToPosition();
    Serial.println("Guide off");

    // ScrewStepper.setMaxSpeed(2000);
    // ScrewStepper.setAcceleration(200);

    // ScrewStepper.moveTo(-500);
    // ScrewStepper.runToPosition();

    // Serial.println("screw pre-homing");

    // ScrewStepper.setMaxSpeed(50);
    // ScrewStepper.setAcceleration(50);

    // initial_homing = 1;
    // while (digitalRead(home_screw)) {
    //   ScrewStepper.moveTo(initial_homing);  // Set the position to move to
    //   initial_homing++;
    //   ScrewStepper.run();

    // }

    // initial_homing = -1;
    // while (!digitalRead(home_screw)) {
    //   ScrewStepper.moveTo(initial_homing);
    //   initial_homing--;
    //   ScrewStepper.run();

    // }

    // ScrewStepper.setCurrentPosition(0);
    // Serial.println("Homing estrusore");

    GuideStepper.moveTo(GuideHoming);
    GuideStepper.runToPosition();
    GuideStepper.setCurrentPosition(0);
    GuideStepper.moveTo(posGuiON);
    GuideStepper.runToPosition();
    Serial.println("Fine ciclo");
  }
}