#include <AccelStepper.h>
#include <EEPROM.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// === OLED ===
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

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

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

// === Costanti meccaniche ===
const float stepPerMM = 50.0;  // passi per millimetro

// === Posizioni salvate ===
const int maxPosizioni = 10;
long posizioni[maxPosizioni];
int posSalvate = 0;
int indiceCorrente = 0;
long posizioneAttuale = 0;
int pausa=10;//pausa per moto continuo

// EEPROM
const int eepromStart = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("init...");

  // OLED Init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 non trovato"));
    while (true);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Sistema pronto");
  display.display();
  delay(1000);

  // Stepper setup
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(200);

  // Pin setup
  pinMode(btnOrario, INPUT_PULLUP);
  pinMode(btnAntiorario, INPUT_PULLUP);
  pinMode(btnSalva, INPUT_PULLUP);
  pinMode(btnVaiA, INPUT_PULLUP);

  // Carica posizioni da EEPROM
  EEPROM.get(eepromStart, posSalvate);
  if (posSalvate < 0 || posSalvate > maxPosizioni) posSalvate = 0;

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

    Serial.println("Sistema pronto");
  }

  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;

  
  // Movimento continuo manuale
  if (orarioPressed) {
    posizioneAttuale += 2;
    stepper.moveTo(posizioneAttuale);
    delay(pausa);
  } else if (antiorarioPressed) {
    posizioneAttuale -= 2;
    stepper.moveTo(posizioneAttuale);
    delay(pausa);
  }

  // RESET lista se Salva + Vai premuti insieme
  if (salvaPressed && vaiPressed) {
    resetListaEEPROM();
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("EEPROM RESET!");
    display.display();
    delay(1000);
    updateDisplay();
    
  Serial.println(" reset");
  }

  // Salvataggio posizione
  static bool salvaPrev = false;
  if (salvaPressed && !salvaPrev && !vaiPressed) {
    if (posSalvate < maxPosizioni) {
      posizioni[posSalvate] = posizioneAttuale;
      posSalvate++;

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

      Serial.print("Salvata posizione: ");
      Serial.println(posizioneAttuale);
      updateDisplay();
    } else {
      Serial.println("Lista piena!");
    }
    delay(300);
  }
  salvaPrev = salvaPressed;

  // Vai alla prossima posizione
  static bool vaiPrev = false;
  if (vaiPressed && !vaiPrev && !salvaPressed && posSalvate > 0) {
    posizioneAttuale = posizioni[indiceCorrente];
    stepper.moveTo(posizioneAttuale);
Serial.print("vai a ");
Serial.print(indiceCorrente);
Serial.print("   Pos:  ");
Serial.println((posizioneAttuale - posizioni[0]) / stepPerMM);

    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Vai a pos #");
    display.print(indiceCorrente);
    display.setCursor(0, 16);
    display.print("Step: ");
    display.print(posizioneAttuale);
    display.setCursor(0, 32);
    display.print("Pos: ");
    display.print((posizioneAttuale - posizioni[0]) / stepPerMM, 2);
    display.println(" mm");
    display.display();

    indiceCorrente = (indiceCorrente + 1) % posSalvate;

    delay(300);
  }
  vaiPrev = vaiPressed;
}

void updateDisplay() {
  float posizioneMM = (posizioneAttuale - posizioni[0]) / stepPerMM;

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Posizioni: ");
  display.println(posSalvate);

  display.setCursor(0, 16);
  display.print("Step: ");
  display.println(posizioneAttuale);

  display.setCursor(0, 32);
  display.print("Pos: ");
  display.print(posizioneMM, 2);
  display.println(" mm");

  display.display();


  Serial.print("Posizioni: ");
  Serial.println(posSalvate);

   Serial.print("Step: ");
  Serial.println(posizioneAttuale);

  Serial.print(posizioneMM, 2);
  Serial.println(" mm");




}

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