#include <AccelStepper.h>
#include <EEPROM.h>
#include <LiquidCrystal.h>

const int rs = 9, en = 10, d4 = 11, d5 = 12, d6 = 13, d7 = A0;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

const int stepPin = 4;
const int dirPin = 3;
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

const int btnOrario     = 5;
const int btnAntiorario = 6;
const int btnSalva      = 7;
const int btnVaiA       = 8;

const float stepPerMM = 100.0;
const int stepIncrement = 100; // Incremento/decremento per i movimenti manuali

const int maxPosizioni = 10;
long posizioni[maxPosizioni]; // Array per memorizzare le posizioni
int posSalvate = 0;           // Numero effettivo di posizioni salvate
int indiceCorrente = 0;       // Indice della *prossima* posizione nel ciclo "Vai"
int indiceVisualizzato = 0;   // Indice della posizione *attualmente* mostrata all'utente

long posizioneAttuale = 0;    // Posizione attuale del motore (in steps)
long offsetPosizione = 3000;  // Offset iniziale o posizione di riferimento

const int eepromStart = 0; // Indirizzo di partenza in EEPROM per i dati

const unsigned long interval = 75; // Intervallo per i movimenti manuali
unsigned long lastMoveTime = 0;
long targetPosition = 0;         // Posizione target per lo stepper
unsigned long lastUpdateDisplay; // Ultimo aggiornamento del display

unsigned long salvaPressStart = 0;
bool salvaAttivo = false; // Flag per il long press del tasto Salva

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 4);
  lcd.print("Sistema pronto");
  delay(1000);

  stepper.setMaxSpeed(2500);    // Velocità massima del motore
  stepper.setAcceleration(1000); // Accelerazione del motore

  // Configurazione dei pin dei pulsanti con pull-up interni
  pinMode(btnOrario, INPUT_PULLUP);
  pinMode(btnAntiorario, INPUT_PULLUP);
  pinMode(btnSalva, INPUT_PULLUP);
  pinMode(btnVaiA, INPUT_PULLUP);

  // Carica il numero di posizioni salvate dalla EEPROM
  EEPROM.get(eepromStart, posSalvate);
  // Validazione: se il valore è fuori range, azzera
  if (posSalvate < 0 || posSalvate > maxPosizioni) {
    posSalvate = 0;
  }

  // Carica le posizioni effettive dalla EEPROM
  for (int i = 0; i < posSalvate; i++) {
    // Le posizioni sono salvate dopo il numero di posizioni (sizeof(int))
    EEPROM.get(eepromStart + sizeof(int) + (i * sizeof(long)), posizioni[i]);
  }

  // Inizializza la posizione attuale e il target
  if (posSalvate == 0) {
    posizioneAttuale = offsetPosizione;
    posizioni[0] = offsetPosizione; // Per visualizzazione iniziale
  } else {
    posizioneAttuale = posizioni[0];
  }

  stepper.setCurrentPosition(posizioneAttuale);
  targetPosition = posizioneAttuale;
  indiceCorrente = 0;       // La prossima piega da richiamare sarà la prima
  indiceVisualizzato = 0;   // Il display mostrerà la prima piega

  lcd.clear();
  updateDisplay(); // Aggiorna il display con lo stato iniziale
}

void loop() {
  stepper.run();

  bool orarioPressed     = digitalRead(btnOrario) == LOW;
  bool antiorarioPressed = digitalRead(btnAntiorario) == LOW;
  bool salvaPressed      = digitalRead(btnSalva) == LOW;
  bool vaiPressed        = digitalRead(btnVaiA) == LOW;

  static bool salvaPrev = false;
  static bool vaiPrev = false;
  static bool salvato = false;

  unsigned long currentMillis = millis();

  // --- Reset della lista se Salva + Vai sono premuti insieme ---
  if (salvaPressed && vaiPressed) {
    resetListaEEPROM();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("EEPROM RESET!");
    delay(1000);
    updateDisplay();
    return;
  }

  // --- Movimento manuale con pulsanti Orario e Antiorario ---
  if (orarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition -= stepIncrement;
    lastMoveTime = currentMillis;
    // Quando si muove manualmente, la posizione attuale è la posizione visualizzata
    indiceVisualizzato = -1; // Flag per indicare che non si è su una piega salvata
                             // o aggiorna indiceVisualizzato alla piega salvata più vicina
                             // ma per ora, -1 indica "non una piega salvata"
    updateDisplay(); // Aggiorna immediatamente il display sulla posizione manuale
  }
  if (antiorarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition += stepIncrement;
    lastMoveTime = currentMillis;
    indiceVisualizzato = -1; // Flag per indicare che non si è su una piega salvata
    updateDisplay(); // Aggiorna immediatamente il display sulla posizione manuale
  }

  // Aggiornamento display quando lo stepper non è in movimento e dopo un certo tempo
  // (utile per visualizzare la posizione finale dopo un moveTo, o per movimenti manuali)
  if (currentMillis - lastUpdateDisplay >= 500 && !stepper.isRunning()) {
    lastUpdateDisplay = currentMillis;
    // Se indiceVisualizzato è -1 (movimento manuale), assicurati che la posizione sia mostrata
    if(indiceVisualizzato == -1) {
        // Puoi trovare la piega più vicina se vuoi, altrimenti mostra solo la posizione
        // Per semplicità, qui mostriamo solo la posizione
        // Se si è fermato su una piega, si potrebbe cercare quale è.
    }
    posizioneAttuale = stepper.currentPosition();
    updateDisplay(); // Ricarica il display per mostrare la posizione attuale
  }

  stepper.moveTo(targetPosition);


  // --- Gestione del pulsante Salva (long press per salvare) ---
  if (salvaPressed && !vaiPressed) {
    if (!salvaPrev) {
      salvaPressStart = currentMillis;
      salvaAttivo = false;
      salvato = false;
    } else if (!salvaAttivo && (currentMillis - salvaPressStart >= 1000)) {
      salvaAttivo = true;

      if (posSalvate < maxPosizioni) {
        posizioni[posSalvate] = stepper.currentPosition();
        posSalvate++;

        EEPROM.put(eepromStart, posSalvate);
        for (int i = 0; i < posSalvate; i++) {
          EEPROM.put(eepromStart + sizeof(int) + (i * sizeof(long)), posizioni[i]);
        }

        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Salvato piega:");
        lcd.print(posSalvate);
        lcd.setCursor(0, 1);
        lcd.print(stepper.currentPosition() / stepPerMM, 2);
        lcd.print(" mm");

        delay(500);
        salvato = true;
        
        // Dopo il salvataggio, l'indice visualizzato è l'ultima piega salvata
        indiceVisualizzato = posSalvate - 1; // Mostra l'ultima piega salvata
        indiceCorrente = 0; // Il prossimo "Vai" partirà dalla prima piega

      } else {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("LISTA PIENA!");
        delay(1000);
      }
    } else if (!salvato) {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("SALVATAGGIO!");
      //lcd.setCursor(0, 1);
      //lcd.print((currentMillis - salvaPressStart) / 1000.0, 1);
      //lcd.print(" sec");
    }
  }
  salvaPrev = salvaPressed;

  // --- Gestione del pulsante Vai (scorre le posizioni salvate) ---
  if (vaiPressed && !vaiPrev && !salvaPressed && posSalvate > 0 && !stepper.isRunning()) {
    // Assicurati che indiceCorrente sia valido prima di usarlo
    if (indiceCorrente >= posSalvate) {
      indiceCorrente = 0; // Torna alla prima posizione se l'indice è fuori range
    }
    
    // Aggiorna l'indice che verrà visualizzato con la posizione a cui stiamo andando
    indiceVisualizzato = indiceCorrente; 

    targetPosition = posizioni[indiceCorrente];
    stepper.moveTo(targetPosition);

    // Aggiorna il display SUBITO per mostrare la piega che sta per essere raggiunta
    updateDisplay(); 

    // Prepara indiceCorrente per la *prossima* pressione del pulsante "Vai"
    indiceCorrente = (indiceCorrente + 1) % posSalvate;

    delay(300);
  }
  vaiPrev = vaiPressed;
}

// --- Funzioni Ausiliarie ---

void updateDisplay() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Piega N:");

  if (posSalvate == 0) {
    lcd.print("0 di 0");
  } else {
    // Se indiceVisualizzato è -1 (movimento manuale o non su una piega salvata), non mostrare indice
    if (indiceVisualizzato == -1) {
        lcd.print("<---->"); // O un altro messaggio che indichi che non è una piega numerata
    } else {
        lcd.print(indiceVisualizzato + 1); // Usa indiceVisualizzato per la stampa
        lcd.print(" di ");
        lcd.print(posSalvate);
    }
  }

  lcd.setCursor(0, 1);
  lcd.print("Pos: ");
  lcd.print(stepper.currentPosition() / stepPerMM, 2);
  lcd.print(" mm");
}

void resetListaEEPROM() {
  posSalvate = 0;
  indiceCorrente = 0;
  indiceVisualizzato = 0; // Reset anche qui

  for (int i = 0; i < maxPosizioni; i++) {
    posizioni[i] = 0;
  }

  EEPROM.put(eepromStart, posSalvate);
}