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

LiquidCrystal_I2C lcd(0x27, 22,21); // Indirizzo I2C standard (0x27 o 0x3F)

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

const int btnOrario     = 12;
const int btnAntiorario = 14;
const int btnSalva      = 27;
const int btnVaiA       = 26;

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;

const int eepromStart = 0;

const unsigned long interval = 150;
unsigned long lastMoveTime = 0;
long targetPosition = 0;

unsigned long salvaPressStart = 0;
bool salvaAttivo = false;

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

  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  pinMode(btnOrario, INPUT_PULLUP);
  pinMode(btnAntiorario, INPUT_PULLUP);
  pinMode(btnSalva, INPUT_PULLUP);
  pinMode(btnVaiA, INPUT_PULLUP);

  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();

  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;

  unsigned long currentMillis = millis();

  if (orarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition += stepIncrement;
    lastMoveTime = currentMillis;
  }
  if (antiorarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition -= stepIncrement;
    lastMoveTime = currentMillis;
  }

  if (!orarioPressed && !antiorarioPressed && targetPosition % stepIncrement != 0) {
    targetPosition = round((float)targetPosition / stepIncrement) * stepIncrement;
  }

  stepper.moveTo(targetPosition);
  posizioneAttuale = targetPosition;

  if (salvaPressed) {
    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 = salvaPressed;

  if (vaiPressed && !vaiPrev && !salvaPressed && posSalvate > 0 && !stepper.isRunning()) {
    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 = vaiPressed;
}

void updateDisplay() {
  float posizioneMM = posizioneAttuale / stepPerMM;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Posizioni: ");
  lcd.print(posSalvate);
  lcd.setCursor(0, 1);
  lcd.print(posizioneMM, 2);
  lcd.print(" mm");
}

void resetListaEEPROM() {
  posSalvate = 1;
  indiceCorrente = 0;
  for (int i = 1; i < maxPosizioni; i++) posizioni[i] = 0;
  posizioni[0] = 0;
  EEPROM.put(eepromStart, posSalvate);
}