#include <AccelStepper.h>

#define MOTOR1_STEP_PIN 2
#define MOTOR1_DIR_PIN 4
#define MOTOR2_STEP_PIN 3
#define MOTOR2_DIR_PIN 5
#define MOTOR1_ENABLE_PIN 6
#define MOTOR2_ENABLE_PIN 7
#define POTENTIOMETER1_PIN A0
#define POTENTIOMETER2_PIN A1
#define START_BUTTON_PIN 8
#define STOP_BUTTON_PIN 9
#define MOTOR1_DIRECTION_BUTTON1_PIN 10
#define MOTOR1_DIRECTION_BUTTON2_PIN 11
#define MOTOR2_DIRECTION_BUTTON1_PIN 12
#define MOTOR2_DIRECTION_BUTTON2_PIN 13
#define LIMIT_SWITCH_PIN A3
#define HOME_BUTTON_PIN A4

AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);

bool isRunning = false;
bool isHoming = false;
bool motor1Direction = true; // True for forward, False for backward
bool motor2Direction = true; // True for forward, False for backward

void setup() {
  pinMode(MOTOR1_ENABLE_PIN, OUTPUT);
  pinMode(MOTOR2_ENABLE_PIN, OUTPUT);
  digitalWrite(MOTOR1_ENABLE_PIN, LOW);
  digitalWrite(MOTOR2_ENABLE_PIN, LOW);

  pinMode(START_BUTTON_PIN, INPUT_PULLUP);
  pinMode(STOP_BUTTON_PIN, INPUT_PULLUP);
  pinMode(MOTOR1_DIRECTION_BUTTON1_PIN, INPUT_PULLUP);
  pinMode(MOTOR1_DIRECTION_BUTTON2_PIN, INPUT_PULLUP);
  pinMode(MOTOR2_DIRECTION_BUTTON1_PIN, INPUT_PULLUP);
  pinMode(MOTOR2_DIRECTION_BUTTON2_PIN, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_PIN, INPUT_PULLUP);
  pinMode(HOME_BUTTON_PIN, INPUT_PULLUP);

  motor1.setMaxSpeed(1000);
  motor2.setMaxSpeed(1000);
}

void loop() {
  // Gestione pulsante START
  if (digitalRead(START_BUTTON_PIN) == LOW && !isRunning) {
    isRunning = true;
    digitalWrite(MOTOR1_ENABLE_PIN, LOW);
    digitalWrite(MOTOR2_ENABLE_PIN, LOW);
    delay(200); // Debounce delay
  }

  // Gestione pulsante STOP
  if (digitalRead(STOP_BUTTON_PIN) == LOW && isRunning) {
    isRunning = false;
    digitalWrite(MOTOR1_ENABLE_PIN, HIGH);
    digitalWrite(MOTOR2_ENABLE_PIN, HIGH);
    delay(200); // Debounce delay
  }

  // Gestione pulsante HOME
  if (digitalRead(HOME_BUTTON_PIN) == LOW && !isRunning) {
    isHoming = true;
    delay(200); // Debounce delay
  }

  // Procedura di homing
  if (isHoming) {
    if (digitalRead(LIMIT_SWITCH_PIN) != LOW) {
      motor1.setSpeed(-500); // Move towards home position
      motor1.runSpeed();
    } else {
      motor1.setCurrentPosition(0);
      isHoming = false;
    }
  } else if (isRunning) {
    // Imposta velocità motori in base ai potenziometri
    motor1.setSpeed(map(analogRead(POTENTIOMETER1_PIN), 0, 1023, 0, 1000) * (motor1Direction ? 1 : -1));
    motor2.setSpeed(map(analogRead(POTENTIOMETER2_PIN), 0, 1023, 0, 1000) * (motor2Direction ? 1 : -1));

    motor1.runSpeed();
    motor2.runSpeed();

    // Controllo del finecorsa e movimento del motore 1
    if (digitalRead(LIMIT_SWITCH_PIN) == LOW) {
      motor1Direction = !motor1Direction; // Invert motor1 direction
      motor1.setSpeed(motor1.speed() * -1); // Reverse direction immediately
      delay(200); // Debounce delay
    }
  } else {
    // Controllo manuale dei pulsanti di direzione per motor1
    if (digitalRead(MOTOR1_DIRECTION_BUTTON1_PIN) == LOW) {
      motor1.setSpeed(-500); // Move backward
      motor1.runSpeed();
    } else if (digitalRead(MOTOR1_DIRECTION_BUTTON2_PIN) == LOW) {
      motor1.setSpeed(500); // Move forward
      motor1.runSpeed();
    } else {
      motor1.stop(); // Stop motor1 if no button is pressed
    }

    // Controllo manuale dei pulsanti di direzione per motor2
    if (digitalRead(MOTOR2_DIRECTION_BUTTON1_PIN) == LOW) {
      motor2.setSpeed(-500); // Move backward
      motor2.runSpeed();
    } else if (digitalRead(MOTOR2_DIRECTION_BUTTON2_PIN) == LOW) {
      motor2.setSpeed(500); // Move forward
      motor2.runSpeed();
    } else {
      motor2.stop(); // Stop motor2 if no button is pressed
    }
  }
}