#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>
#include <TM1637Display.h> // Aggiunta per il display TM1637

// Definizione dei pin
#define buttonSelect 8
#define buttonIncrement 9
#define buttonDecrement 10
#define buttonStart 11
#define buttonStop 12
#define buttonHoming A1  // Pulsante di homing su A1

// Pin per il motore A
#define enablePinA 7
#define stepPinA 6
#define dirPinA 5

// Pin per il motore B
#define enablePinB 4
#define stepPinB 3
#define dirPinB 2

// Pin per il potenziometro
#define potPin A6  // Modificato: Pin analogico per il potenziometro da A0 a A6

// Pin per il finecorsa
#define finecorsaPin 13  // Usa un pin digitale per il finecorsa (es. D13)

// LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// TM1637
#define CLK  A2 // Pin di clock per il TM1637
#define DIO  A3 // Pin di data per il TM1637
TM1637Display display(CLK, DIO);

// Crea un'istanza di AccelStepper per ciascun motore
AccelStepper motorA(AccelStepper::DRIVER, stepPinA, dirPinA);
AccelStepper motorB(AccelStepper::DRIVER, stepPinB, dirPinB);

// Variabili per i parametri
float spessoreFilo = 0.500;  // Spessore del filo in mm
int numGiri = 0;             // Numero di giri impostati
int giriRestanti = 0;        // Giri rimanenti da eseguire
int parametroSelezionato = 0; // 0: Seleziona Giri, 1: Seleziona Spessore
bool motoriAttivi = false;   // Stato dei motori
int direzioneA = 1;          // 1: avanti, -1: indietro
bool homingInCorso = false;  // Stato dell'homing

// Variabili per la gestione dei pulsanti
unsigned long lastIncrementTime = 0; // Ultimo tempo di incremento
unsigned long lastDecrementTime = 0; // Ultimo tempo di decremento
const unsigned long incrementDelay = 50; // Tempo per incremento rapido
const unsigned long decrementDelay = 50; // Tempo per decremento rapido
const int quickIncrementValue = 10; // Valore di incremento veloce per i giri

void setup() {
  pinMode(buttonSelect, INPUT_PULLUP);
  pinMode(buttonIncrement, INPUT_PULLUP);
  pinMode(buttonDecrement, INPUT_PULLUP);
  pinMode(buttonStart, INPUT_PULLUP);
  pinMode(buttonStop, INPUT_PULLUP);
  pinMode(buttonHoming, INPUT_PULLUP); // Pulsante di homing

  pinMode(enablePinA, OUTPUT);
  pinMode(enablePinB, OUTPUT);
  digitalWrite(enablePinA, HIGH); // Disabilita il motore A
  digitalWrite(enablePinB, HIGH); // Disabilita il motore B

  pinMode(finecorsaPin, INPUT_PULLUP); // Finecorsa con pull-up interno

  lcd.init();
  lcd.backlight();
  
  display.setBrightness(0x0f); // Imposta la luminosità massima del TM1637
  aggiornaLCD(); // Aggiorna il display LCD

  motorA.setMaxSpeed(2000);  // Velocità massima iniziale per il motore A
  motorA.setAcceleration(1000); // Accelerazione iniziale per il motore A
  motorB.setMaxSpeed(2000);  // Velocità massima iniziale per il motore B
  motorB.setAcceleration(1000); // Accelerazione iniziale per il motore B
}

void loop() {
  // Leggi il valore del potenziometro e mappa il valore per la velocità
  int potValue = analogRead(potPin);
  int speedValue = map(potValue, 0, 1023, 0, 4000);  // Aumenta il limite massimo di velocità

  // Imposta la velocità massima dei motori in base al potenziometro
  motorA.setMaxSpeed(speedValue);
  motorB.setMaxSpeed(speedValue);

  // Seleziona tra giri e spessore
  if (digitalRead(buttonSelect) == LOW) {
    parametroSelezionato = (parametroSelezionato + 1) % 2;  // Cambia tra 0 (Giri) e 1 (Spessore)
    aggiornaLCD();
    delay(200); // Debounce
  }

  // Incremento e decremento del parametro selezionato
  if (parametroSelezionato == 0) { // Se stiamo modificando i giri
    if (digitalRead(buttonIncrement) == LOW) {
      // Incremento normale
      numGiri++;  // Incrementa il numero di giri
      aggiornaLCD();
      delay(200); // Debounce
    }

    if (digitalRead(buttonDecrement) == LOW) {
      if (numGiri > 0) numGiri--;  // Decrementa il numero di giri
      aggiornaLCD();
      delay(200); // Debounce
    }

    // Incremento rapido
    if (digitalRead(buttonIncrement) == LOW) {
      unsigned long currentTime = millis();
      if (currentTime - lastIncrementTime > incrementDelay) {
        numGiri += quickIncrementValue; // Incremento veloce
        aggiornaLCD();
        lastIncrementTime = currentTime; // Aggiorna il tempo dell'ultimo incremento
      }
    } else {
      lastIncrementTime = millis(); // Resetta il tempo se il pulsante non è premuto
    }

    // Decremento rapido
    if (digitalRead(buttonDecrement) == LOW) {
      unsigned long currentTime = millis();
      if (currentTime - lastDecrementTime > decrementDelay) {
        if (numGiri > quickIncrementValue) numGiri -= quickIncrementValue;  // Decrementa velocemente
        aggiornaLCD();
        lastDecrementTime = currentTime; // Aggiorna il tempo dell'ultimo decremento
      }
    } else {
      lastDecrementTime = millis(); // Resetta il tempo se il pulsante non è premuto
    }

  } else { // Se stiamo modificando lo spessore
    if (digitalRead(buttonIncrement) == LOW) {
      unsigned long currentTime = millis();
      if (currentTime - lastIncrementTime > incrementDelay) {
        spessoreFilo += 0.001;  // Incrementa lo spessore di 0.001mm
        aggiornaLCD();
        lastIncrementTime = currentTime; // Aggiorna il tempo dell'ultimo incremento
      }
    } else {
      lastIncrementTime = millis(); // Resetta il tempo se il pulsante non è premuto
    }

    if (digitalRead(buttonDecrement) == LOW) {
      unsigned long currentTime = millis();
      if (currentTime - lastDecrementTime > decrementDelay) {
        if (spessoreFilo > 0.001) spessoreFilo -= 0.001;  // Decrementa lo spessore di 0.001mm
        aggiornaLCD();
        lastDecrementTime = currentTime; // Aggiorna il tempo dell'ultimo decremento
      }
    } else {
      lastDecrementTime = millis(); // Resetta il tempo se il pulsante non è premuto
    }
  }

  // Pulsante di homing
  if (digitalRead(buttonHoming) == LOW) {
    // Abilita il motore A
    digitalWrite(enablePinA, LOW);
    motorA.setSpeed(400 * (direzioneA)); // Imposta la velocità di homing in base alla direzione
    homingInCorso = true; // Inizia l'homing

    while (homingInCorso) {
      motorA.runSpeed(); // Muovi il motore A

      // Controllo del finecorsa
      if (digitalRead(finecorsaPin) == LOW) { // Finecorsa attivato
        motorA.stop(); // Ferma il motore A
        delay(200); // Attendi un attimo prima di riprendere
        motorA.setCurrentPosition(0); // Resetta la posizione del motore A
        homingInCorso = false; // Ferma l'homing
        direzioneA *= -1; // Cambia la direzione per il prossimo homing
        break; // Esci dal ciclo
      }
    }
    delay(200); // Debounce
  }

  // Inizio movimento quando si preme il tasto start
  if (digitalRead(buttonStart) == LOW && !motoriAttivi) {
    motoriAttivi = true;
    giriRestanti = numGiri; // Imposta i giri rimanenti al valore attuale
    digitalWrite(enablePinA, LOW); // Abilita il motore A
    digitalWrite(enablePinB, LOW); // Abilita il motore B
    motorA.setCurrentPosition(0); // Resetta la posizione del motore A
    delay(200);
  }

  // Stop del movimento quando si preme il tasto stop
  if (digitalRead(buttonStop) == LOW) {
    motoriAttivi = false; // Disabilita il movimento
    motorA.stop(); // Ferma il motore A
    motorB.stop(); // Ferma il motore B
    digitalWrite(enablePinA, HIGH); // Disabilita il motore A
    digitalWrite(enablePinB, HIGH); // Disabilita il motore B
    delay(200);
  }

  // Gestione del movimento dei motori
  if (motoriAttivi && giriRestanti > 0) {
    int passiPerGiroB = 200; // Passi per un giro del motore B (1 giro = 200 step)
    int passiPerGiroA = 800; // Passi per un giro completo del motore A (1 giro = 800 step)
    float spostamentoA_perGiroB = (spessoreFilo / 8.0) * passiPerGiroA;  // Step del motore A per spostamento proporzionale

    // Muovi il motore B di esattamente 200 step (1 giro completo)
    motorB.moveTo(motorB.currentPosition() + passiPerGiroB);
    motorA.moveTo(motorA.currentPosition() + (spostamentoA_perGiroB * direzioneA)); // Applica la direzione

    // Esegui il movimento finché entrambi i motori non raggiungono la destinazione
    while (motorB.distanceToGo() != 0 || motorA.distanceToGo() != 0) {
      // Controllo del finecorsa
      if (digitalRead(finecorsaPin) == LOW) {  // Se il finecorsa è attivo
        motorA.stop(); // Ferma il motore A
        delay(200); // Attendi un attimo prima di riprendere

        // Cambia la direzione del motore A
        direzioneA *= -1; // Inverti la direzione
        motorA.setCurrentPosition(0); // Resetta la posizione del motore A

        // Muovi nella nuova direzione
        motorA.moveTo(motorA.currentPosition() + (spostamentoA_perGiroB * direzioneA)); // Muovi nella nuova direzione
      }

      // Esegui i motori
      motorA.run();
      motorB.run();
    }

    giriRestanti--; // Decrementa i giri rimanenti
    aggiornaLCD();  // Aggiorna l'LCD
  }

  // Aggiornamento del display TM1637
  display.showNumberDec(numGiri, false); // Mostra il numero di giri sul TM1637
}

// Funzione per aggiornare l'LCD
void aggiornaLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);

  // Visualizza giri con evidenza sul parametro selezionato
  if (parametroSelezionato == 0) {
    lcd.print(">Giri: ");
  } else {
    lcd.print(" Giri: ");
  }
  lcd.print(numGiri);

  lcd.setCursor(0, 1);

  // Visualizza spessore con evidenza sul parametro selezionato
  if (parametroSelezionato == 1) {
    lcd.print(">Spessore: ");
  } else {
    lcd.print(" Spessore: ");
  }
  lcd.print(spessoreFilo, 3);  // Mostra tre cifre decimali
}