#include <AccelStepper.h>
#include <EEPROM.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const unsigned long interval = 75; // Intervallo per i movimenti manuali
const unsigned long velocitaMax = 2500; // massima velocita
const unsigned long accelerazione = 1000; // accelerazione
const unsigned int attesa = 4000; // Intervallo attesa piegatura

// --- Debounce Constants ---
const unsigned long debounceDelay = 50; // Tempo di debounce in ms

// --- Debounce Variables ---
int btnOrarioState;
int lastBtnOrarioState = HIGH;
unsigned long lastBtnOrarioChange = 0;

int btnAntiorarioState;
int lastBtnAntiorarioState = HIGH;
unsigned long lastBtnAntiorarioChange = 0;

int btnSalvaState;
int lastBtnSalvaState = HIGH;
unsigned long lastBtnSalvaChange = 0;

int btnVaiAState;
int lastBtnVaiAState = HIGH;
unsigned long lastBtnVaiAChange = 0;

// Variabili per lo stato debounced dei pulsanti
bool orarioPressedDebounced = false;
bool antiorarioPressedDebounced = false;
bool salvaPressedDebounced = false;
bool vaiPressedDebounced = false;

LiquidCrystal_I2C lcd(0x27, 20,4); // Indirizzo I2C standard (0x27 o 0x3F)

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

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.init();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("Sistema pronto");
    delay(1000);

    stepper.setMaxSpeed(velocitaMax);    // Velocità massima del motore
    stepper.setAcceleration(accelerazione); // 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();
    checkButtons(); // Controlla e aggiorna lo stato debounced dei pulsanti

    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 (salvaPressedDebounced && vaiPressedDebounced) {
        resetListaEEPROM();
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("EEPROM RESET!");
        delay(1000);
        updateDisplay();
        return;
    }

    // --- Movimento manuale con pulsanti Orario e Antiorario ---
    if (orarioPressedDebounced && 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
    }
    if (antiorarioPressedDebounced && currentMillis - lastMoveTime >= interval) {
        targetPosition += stepIncrement;
        lastMoveTime = currentMillis;
        indiceVisualizzato = -1; // Flag per indicare che non si è su una piega salvata
    }

    // Aggiornamento display quando lo stepper non è in movimento e dopo un certo tempo
    if (currentMillis - lastUpdateDisplay >= 500 && !stepper.isRunning()) {
        lastUpdateDisplay = currentMillis;
        posizioneAttuale = stepper.currentPosition();
        updateDisplay(); // Ricarica il display per mostrare la posizione attuale
    }

    stepper.moveTo(targetPosition);

    // --- Gestione del pulsante Salva (long press per salvare) ---
    if (salvaPressedDebounced) {
        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("salva...");
            lcd.setCursor(0, 1);
            lcd.print((currentMillis - salvaPressStart) / 1000.0, 1);
            lcd.print(" sec");
        }
    }
    salvaPrev = salvaPressedDebounced;

    // --- Gestione del pulsante Vai (scorre le posizioni salvate) ---
    if (vaiPressedDebounced && !vaiPrev && 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
        }
        //attesa
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("attesa.");
          lcd.print(attesa/1000);
          lcd.print("..sec..");  
        delay(attesa);
        // 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 = vaiPressedDebounced;
}

// --- Funzione per il Debounce dei pulsanti ---
void checkButtons() {
    unsigned long currentMillis = millis();

    // Debounce per btnOrario
    int readingOrario = digitalRead(btnOrario);
    if (readingOrario != lastBtnOrarioState) {
        lastBtnOrarioChange = currentMillis;
    }
    if (currentMillis - lastBtnOrarioChange > debounceDelay) {
        if (readingOrario != btnOrarioState) {
            btnOrarioState = readingOrario;
            if (btnOrarioState == LOW) {
                orarioPressedDebounced = true;
            } else {
                orarioPressedDebounced = false;
            }
        }
    }
    lastBtnOrarioState = readingOrario;

    // Debounce per btnAntiorario
    int readingAntiorario = digitalRead(btnAntiorario);
    if (readingAntiorario != lastBtnAntiorarioState) {
        lastBtnAntiorarioChange = currentMillis;
    }
    if (currentMillis - lastBtnAntiorarioChange > debounceDelay) {
        if (readingAntiorario != btnAntiorarioState) {
            btnAntiorarioState = readingAntiorario;
            if (btnAntiorarioState == LOW) {
                antiorarioPressedDebounced = true;
            } else {
                antiorarioPressedDebounced = false;
            }
        }
    }
    lastBtnAntiorarioState = readingAntiorario;

    // Debounce per btnSalva
    int readingSalva = digitalRead(btnSalva);
    if (readingSalva != lastBtnSalvaState) {
        lastBtnSalvaChange = currentMillis;
    }
    if (currentMillis - lastBtnSalvaChange > debounceDelay) {
        if (readingSalva != btnSalvaState) {
            btnSalvaState = readingSalva;
            salvaPressedDebounced = (btnSalvaState == LOW);
        }
    }
    lastBtnSalvaState = readingSalva;

    // Debounce per btnVaiA
    int readingVaiA = digitalRead(btnVaiA);
    if (readingVaiA != lastBtnVaiAState) {
        lastBtnVaiAChange = currentMillis;
    }
    if (currentMillis - lastBtnVaiAChange > debounceDelay) {
        if (readingVaiA != btnVaiAState) {
            btnVaiAState = readingVaiA;
            vaiPressedDebounced = (btnVaiAState == LOW);
        }
    }
    lastBtnVaiAState = readingVaiA;
}

// --- 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);
}