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


const int attesa_piega=4000;
const int maxspeed=1000;
const int  acceleration=500;


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

const float stepPerMM = 50.0;
const int stepIncrement = 25;
const int maxPosizioni = 10;
long posizioni[maxPosizioni];
int posSalvate = 1;
int indiceCorrente = 0;
long posizioneAttuale = 0;
long targetPosition = 0;
const int eepromStart = 0;

unsigned long lastMoveTime = 0;
const unsigned long interval = 150;
unsigned long salvaPressStart = 0;
bool salvaAttivo = false;

LiquidCrystal_I2C lcd(0x27, 20, 4);

class DebouncedButton {
  int pin;
  bool state;
  bool lastStableState;
  unsigned long lastDebounceTime;
  const unsigned long debounceDelay = 50;

public:
  DebouncedButton(int p) : pin(p), state(false), lastStableState(false), lastDebounceTime(0) {}

  void setup() {
    pinMode(pin, INPUT_PULLUP);
    state = !digitalRead(pin);
    lastStableState = state;
    lastDebounceTime = millis();
  }

  void update() {
    bool reading = !digitalRead(pin);
    if (reading != lastStableState) {
      lastDebounceTime = millis();
    }
    if ((millis() - lastDebounceTime) > debounceDelay) {
      if (reading != state) {
        state = reading;
      }
    }
    lastStableState = reading;
  }

  bool isPressed() {
    return state;
  }
};

DebouncedButton buttonOrario(5);
DebouncedButton buttonAntiorario(6);
DebouncedButton buttonSalva(7);
DebouncedButton buttonVaiA(8);

void updateDisplay() {
  float posizioneMM = posizioneAttuale / stepPerMM;
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Posizioni: ");
  lcd.print(posSalvate);
  lcd.setCursor(0, 1);
  lcd.print("Pos: ");
  lcd.print(posizioneMM, 2);
  lcd.print(" mm");
}

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Sistema pronto");
  delay(1000);

  stepper.setMaxSpeed(maxspeed);
  stepper.setAcceleration(acceleration);

  buttonOrario.setup();
  buttonAntiorario.setup();
  buttonSalva.setup();
  buttonVaiA.setup();

  EEPROM.get(eepromStart, posSalvate);
  if (posSalvate < 1 || posSalvate > maxPosizioni) posSalvate = 1;

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

  targetPosition = posizioneAttuale;
  updateDisplay();
}

void loop() {
  stepper.run();

  buttonOrario.update();
  buttonAntiorario.update();
  buttonSalva.update();
  buttonVaiA.update();

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

  unsigned long currentMillis = millis();

  if (buttonOrario.isPressed() && currentMillis - lastMoveTime >= interval) {
    targetPosition += stepIncrement;
    lastMoveTime = currentMillis;
  }
  if (buttonAntiorario.isPressed() && currentMillis - lastMoveTime >= interval) {
    targetPosition -= stepIncrement;
    lastMoveTime = currentMillis;
  }

  if (!buttonOrario.isPressed() && !buttonAntiorario.isPressed() && targetPosition % stepIncrement != 0) {
    targetPosition = round((float)targetPosition / stepIncrement) * stepIncrement;
  }

  stepper.moveTo(targetPosition);
  posizioneAttuale = targetPosition;

  if (buttonSalva.isPressed()) {
    if (!salvaPrev) {
      salvaPressStart = currentMillis;
      salvaAttivo = false;
    } else if (!salvaAttivo && (currentMillis - salvaPressStart >= 1000)) {
      salvaAttivo = true;
      if (posSalvate < maxPosizioni) {
        posizioni[posSalvate] = posizioneAttuale;
        posSalvate++;
        EEPROM.put(eepromStart, posSalvate);
        for (int i = 1; i < posSalvate; i++) {
          EEPROM.put(eepromStart + sizeof(int) + (i - 1) * sizeof(long), posizioni[i]);
        }
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Salvato:");
        lcd.setCursor(0, 1);
        lcd.print(posizioneAttuale / stepPerMM, 2);
        lcd.print(" mm");
        delay(500);
        updateDisplay();
      }
    } else {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Tieni premuto");
      lcd.setCursor(0, 1);
      lcd.print((currentMillis - salvaPressStart) / 1000.0, 1);
      lcd.print(" sec");
    }
  }
  salvaPrev = buttonSalva.isPressed();

  if (buttonVaiA.isPressed() && !vaiPrev && !buttonSalva.isPressed() && posSalvate > 0 && !stepper.isRunning()) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("ATTESA..");
    lcd.print(attesa_piega);
    lcd.print(" millisec.");
    delay(attesa_piega);
    
    
    
    targetPosition = posizioni[indiceCorrente];
    posizioneAttuale = targetPosition;
    stepper.moveTo(targetPosition);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Piega N:");
    lcd.print(indiceCorrente + 1);
    lcd.setCursor(0, 1);
    lcd.print("Pos: ");
    lcd.print(posizioneAttuale / stepPerMM, 2);
    lcd.print(" mm");
    indiceCorrente = (indiceCorrente + 1) % posSalvate;
    delay(300);
  }
  vaiPrev = buttonVaiA.isPressed();
}