#include "AccelStepper.h"

#define home_switch1 8 // Pin 8 connected to Home Switch (MicroSwitch)
#define home_switch2 7
#define LEDPin 13

int buttonPin = 6;
int state = LOW;
int state2 = LOW;
int btnState = 0;
int btn;
int motor1;
int motor2;
int btnStatePrevious = LOW;

bool btnStateLongPress = false;

unsigned long btnLongPressMillis;
unsigned long previousBtnMillis;
unsigned long btnPressDuration;
unsigned long currentMillis;
unsigned long time = millis();
unsigned long minBtnLongPressDuration = 3000;
const int intervalBtn = 50;

#define HOMEING 100 //Speed
#define HOMEING_RETURN 20 //Return Speed 
#define HOMEING_ACCELERATION 200

#define STEPS1 400  // Right Joystick pos 1
#define STEPS2 200  // Left Joystick pos 1
#define STEPS3 600  // Right Joystick pos 2
#define STEPS4 0  // Left Joystick pos 2
#define MAXSPEED 1000
#define ACCELERATION 800
#define MICROSTEP1 8 * STEPS1 // Full, Halv, 1/4, 1/8, 1/16
#define MICROSTEP2 8 * STEPS2
#define MICROSTEP3 8 * STEPS3
#define MICROSTEP4 8 * STEPS4

AccelStepper stepper1(1, 5, 2);   // 1 = Easy Driver, 5 Step Pin, 2 Dir Pin.
AccelStepper stepper2(1, 4, 3);


long initial_homing1 = -1; // Used to Home Stepper at startup
long initial_homing2 = -1;

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

  pinMode(home_switch1, INPUT);
  pinMode(home_switch2, INPUT);
  pinMode(buttonPin, INPUT);
  pinMode(LEDPin, OUTPUT);

  delay(5);  // Wait for EasyDriver wake up

  stepper1.setMaxSpeed(HOMEING);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepper1.setAcceleration(HOMEING_ACCELERATION);  // Set Acceleration of Stepper
  stepper2.setMaxSpeed(HOMEING);
  stepper2.setAcceleration(HOMEING_ACCELERATION);

  // Start Homing procedure of Stepper Motor at startup
  Serial.print("Stepper is Homing . . . . . . . . . . . ");

  while (digitalRead(home_switch1)) {  // Make the Stepper move CCW until the switch is activated
    stepper1.moveTo(initial_homing1);  // Set the position to move to
    initial_homing1--;  // Decrease by 1 for next move if needed
    stepper1.run();  // Start moving the stepper

    delay(5);
  }

  stepper1.setCurrentPosition(0);  // Set the current position as zero for now
  stepper1.setMaxSpeed(HOMEING_RETURN);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepper1.setAcceleration(HOMEING_ACCELERATION);  // Set Acceleration of Stepper
  initial_homing1 = 1;

  while (!digitalRead(home_switch1)) { // Make the Stepper move CW until the switch is deactivated
    stepper1.moveTo(initial_homing1);
    initial_homing1++;
    stepper1.run();
    delay(5);
  }

  stepper1.setCurrentPosition(0);
  Serial.println("Homing Completed");
  Serial.println("");
  stepper1.setMaxSpeed(MAXSPEED);      // Set Max Speed of Stepper (Faster for regular movements)
  stepper1.setAcceleration(ACCELERATION);  // Set Acceleration of Stepper

  while (digitalRead(home_switch2)) {  // Make the Stepper move CCW until the switch is activated
    stepper2.moveTo(initial_homing2);  // Set the position to move to
    initial_homing2--;  // Decrease by 1 for next move if needed
    stepper2.run();  // Start moving the stepper

    delay(5);
  }

  stepper2.setCurrentPosition(0);  // Set the current position as zero for now
  stepper2.setMaxSpeed(HOMEING_RETURN);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepper2.setAcceleration(HOMEING_ACCELERATION);  // Set Acceleration of Stepper
  initial_homing2 = 1;

  while (!digitalRead(home_switch2)) { // Make the Stepper move CW until the switch is deactivated
    stepper2.moveTo(initial_homing2);
    stepper2.run();
    initial_homing2++;
    delay(5);
  }
  stepper2.setCurrentPosition(0);

  stepper2.setMaxSpeed(MAXSPEED);// Set Max Speed of Stepper (Faster for regular movements)
  stepper2.setAcceleration(ACCELERATION);// Set Acceleration of Stepper

}

void readBtnState()
{
  if (currentMillis - previousBtnMillis > intervalBtn)
  {
    int btn = digitalRead(buttonPin);
    {
      if (btn == HIGH && btnStatePrevious == LOW && !btnStateLongPress)
      {
        btnLongPressMillis = currentMillis;
        btnStatePrevious = HIGH;
        Serial.println("Button pressed");

        if (state == HIGH)
        {
          motor1 = 0;
          state = LOW;
        }
        else
        {
          motor1 = 1;
          state = HIGH;
        }

      }
      digitalWrite(LEDPin, state);
    }

    btnPressDuration = currentMillis - btnLongPressMillis;


    if (btn == HIGH && !btnStateLongPress && btnPressDuration >= minBtnLongPressDuration)
    {
      btnStateLongPress = true;

      if (state2 == HIGH)
      {
        motor2 = 0;
        state2 = LOW;
      }
      else
      {
        motor2 = 1;
        state2 = HIGH;
      }


      Serial.println("Button long pressed");
    }

    if (btn == LOW && btnStatePrevious == HIGH)
    {
      btnStatePrevious = LOW;
      btnStateLongPress = false;
      Serial.println("Button released");

      if (btnPressDuration < minBtnLongPressDuration)
      {
        Serial.println("Button pressed shortly");
      }
    }
    previousBtnMillis = currentMillis;
  }
}

void loop()
{

  currentMillis = millis();
  readBtnState();


   if (motor1 == 0 && motor2 == 0 )  // This line should only work in Short Press 
   {                                 // This line Should not Run if you Long Press
       stepper1.moveTo(0);
       stepper2.moveTo(0);
       stepper1.run();
       stepper2.run();
   }

  if (motor1 == 1 && motor2 == 0 )
  {
    stepper2.moveTo(MICROSTEP2); // Left stepper to 200
    stepper2.run();
    if (motor1 == 1 && motor2 == 0)
    {
      stepper1.moveTo(MICROSTEP1); //Right stepper to 400
      stepper1.run();
    }
  }
  else if (motor1 == 1 && motor2 == 1 )
  {
    stepper1.moveTo(MICROSTEP3); //Right stepper to 600
    stepper1.run();
    if (motor1 == 1 && motor2 == 1) // Left stepper to home position
    {
      stepper2.moveTo(MICROSTEP4);
      stepper2.run();
    }
  }
  else if (motor1 == 0 && motor2 == 1 )  // Left stepper to 200
  {
    stepper2.moveTo(MICROSTEP2);
    stepper2.run();
    if (motor1 == 1 && motor2 == 1) //Left stepper to home position
    {
      stepper2.moveTo(MICROSTEP4);
      stepper2.run();
    }
  }
}