#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;

const int maxPosizioni = 10;
long posizioni[maxPosizioni];
int posSalvate = 1;
int indiceCorrente = 0;
long posizioneAttuale = 0;
long offsetPosizione = 0;

const int eepromStart = 0;

const unsigned long interval = 75;
unsigned long lastMoveTime = 0;
long targetPosition = 0;
unsigned long lastUpdateDisplay;

unsigned long salvaPressStart = 0;
bool salvaAttivo = false;

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

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

  pinMode(btnOrario, INPUT_PULLUP);
  pinMode(btnAntiorario, INPUT_PULLUP);
  pinMode(btnSalva, INPUT_PULLUP);
  pinMode(btnVaiA, INPUT_PULLUP);
 offsetPosizione=3000;
  EEPROM.get(eepromStart, posSalvate);
  if (posSalvate < 1 || posSalvate > maxPosizioni) posSalvate = 1;

  posizioni[0] = offsetPosizione;
  for (int i = 1; i < posSalvate; i++) {
    EEPROM.get(eepromStart + sizeof(int) + (i - 1) * sizeof(long), posizioni[i]);
  }
  offsetPosizione=3000;
  posizioneAttuale=offsetPosizione;
  stepper.setCurrentPosition(posizioneAttuale);
  targetPosition = posizioneAttuale;
  
 
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Tot. Pos.: ");
  lcd.print(posSalvate);
  lcd.setCursor(0, 1);
  lcd.print(posizioneAttuale / stepPerMM, 2);
  lcd.print(" mm");;
}

void loop() {
  stepper.run();

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


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

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

  unsigned long currentMillis = millis();
  


  if (orarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition -= stepIncrement;
    lastMoveTime = currentMillis;
   // move=true;
  }
  if (antiorarioPressed && currentMillis - lastMoveTime >= interval) {
    targetPosition += stepIncrement;
    lastMoveTime = currentMillis;
    //move=true;
  }
 // Serial.print("curr:");
//Serial.print(stepper.currentPosition());
//Serial.print("att:");
//Serial.print(posizioneAttuale);
//Serial.print("    tar:");
//Serial.println(targetPosition);
  //if (!orarioPressed && !antiorarioPressed && targetPosition % stepIncrement != 0) {
  //  targetPosition = round((float)targetPosition / stepIncrement) * stepIncrement;
 // updateDisplay();
 // }
 if (currentMillis - lastUpdateDisplay >= 500 && !stepper.isRunning()) {
    
  lastUpdateDisplay = currentMillis;
   updateDisplay();
  }
  //if (move && !stepper.isRunning()){
 // updateDisplay();
 // move=false;}
   
  stepper.moveTo(targetPosition);
  posizioneAttuale = targetPosition;

  if (salvaPressed  && !vaiPressed) {
    if (!salvaPrev) {
      salvaPressStart = currentMillis;
      salvaAttivo = false;
      salvato = 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 piega:");
        lcd.print(posSalvate);
        lcd.setCursor(0, 1);
        lcd.print(posizioneAttuale / stepPerMM, 2);
        lcd.print(" mm");

        delay(500);
        salvato = true;
        indiceCorrente=posSalvate-1;
       //dopo salvato il ciclo pieghe deve ripartire dalla prima
      }
    } 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");
    }else{
 updateDisplay();
  }
  }
  salvaPrev = salvaPressed;

  if (vaiPressed && !vaiPrev && !salvaPressed && posSalvate > 0 && !stepper.isRunning()) {
    if (indiceCorrente==posSalvate) indiceCorrente=0;
    targetPosition = posizioni[indiceCorrente+1];
    posizioneAttuale = targetPosition;
    stepper.moveTo(targetPosition);

    //lcd.setCursor(0, 0);
    //lcd.print("Piega N:");
    //lcd.print(indiceCorrente + 1);
    //lcd.print(" di ");
    //lcd.print(posSalvate);

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

    indiceCorrente = (indiceCorrente) % posSalvate;
    updateDisplay();
    delay(300);
  }
  vaiPrev = vaiPressed;
}
//update display piega corrente
void updateDisplay() {

  lcd.clear();
    lcd.setCursor(0, 0);
    //lcd.print("POSIZIONE:");
    lcd.print("Piega N:");
    lcd.print(indiceCorrente+1);
    lcd.print(" di ");
    lcd.print(posSalvate);

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

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