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

// Configurazione LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Pin Pulsanti
const int btn1 = 2;  // Scorrere i menu
const int btn2 = 3;  // Modifica valori (incrementa)
const int btn3 = 4;  // Modifica valori (decrementa)
const int btn4 = 10; // Avanza il motore e incrementa contatore

// Pin Motore A4988
#define stepPin 5
#define dirPin 6
#define ms1 7
#define ms2 8
#define ms3 9

// Variabili per la gestione dei menu
int menuIndex = 0;           // Indice del menu attuale
float mmValue = 30.0;         // Valore per "mm"
int microstepIndex = 4;      // Indice per il microstepping
int contatorePezzi = 0;      // Contatore pezzi
int pezziTarget = 10;         // Quantità target impostata
float velocita = 50.0;        // Velocità in mm/s
float accelerazione = 30.0;   // Accelerazione in mm/s²
bool inMenu = true;          // Stato del menu

// Valori per i microstepping
const char* microstepOptions[] = {"Full-step", "Half-step", "1/4-step", "1/8-step", "1/16-step"};
const int msSettings[][3] = {
  {LOW, LOW, LOW},  // Full-step
  {HIGH, LOW, LOW}, // Half-step
  {LOW, HIGH, LOW}, // 1/4-step
  {HIGH, HIGH, LOW},// 1/8-step
  {HIGH, HIGH, HIGH}// 1/16-step
};

// Costante per la circonferenza della ruota
const float CIRCUMFERENCE = 11 * 3.1416; // Circonferenza in mm

// AccelStepper setup
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// Funzione per controllare i pulsanti
bool buttonPressed(int pin) {
  return digitalRead(pin) == LOW;
}

void setup() {
  lcd.init();
  lcd.backlight();

  pinMode(btn1, INPUT_PULLUP);
  pinMode(btn2, INPUT_PULLUP);
  pinMode(btn3, INPUT_PULLUP);
  pinMode(btn4, INPUT_PULLUP);

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(ms1, OUTPUT);
  pinMode(ms2, OUTPUT);
  pinMode(ms3, OUTPUT);

  stepper.setMaxSpeed(1000);  // Velocità massima del motore (da regolare)
  stepper.setAcceleration(500); // Accelerazione del motore

  lcd.setCursor(0, 0);
  lcd.print("Inizializzazione");
  delay(1000);
  lcd.clear();
}

void loop() {
  if (inMenu) {
    displayMenu();
    handleMenu();
  } else {
    displaySettings();
    if (buttonPressed(btn4)) {
      eseguiAvanzamento();
    }
  }
}

void displayMenu() {
  lcd.clear();
  switch (menuIndex) {
    case 0:
      lcd.print("Lunghezza cavo: ");
      lcd.setCursor(0, 1);
      lcd.print(mmValue, 1);  // Mostra il valore in mm con 1 decimale
      break;
    case 1:
      lcd.print("Microstep: ");
      lcd.setCursor(0, 1);
      lcd.print(microstepOptions[microstepIndex]);
      break;
    case 2:
      lcd.print("Velocita: ");
      lcd.setCursor(0, 1);
      lcd.print(velocita, 1);
      lcd.print(" mm/s");
      break;
    case 3:
      lcd.print("Acceleraz: ");
      lcd.setCursor(0, 1);
      lcd.print(accelerazione, 1);
      lcd.print(" mm/s2");
      break;
    case 4:
      lcd.print("Contatore: ");
      lcd.setCursor(0, 1);
      lcd.print(pezziTarget);
      break;
  }
}

void handleMenu() {
  if (buttonPressed(btn1)) {
    menuIndex = (menuIndex + 1) % 5;  // Aggiunto menu accelerazione
    delay(300);  // Debounce
  }

  if (buttonPressed(btn2)) {
    switch (menuIndex) {
      case 0: mmValue += 0.1; break;  // Incrementa di 0.1mm
      case 1: microstepIndex = (microstepIndex + 1) % 5; break;  // Scorri tra i microstep
      case 2: velocita += 10; break;  // Incrementa velocità di 10 mm/s
      case 3: accelerazione += 10; break;  // Incrementa accelerazione di 10 mm/s²
      case 4: pezziTarget++; break;  // Incrementa il contatore
    }
    delay(100);
  }

  if (buttonPressed(btn3)) {
    switch (menuIndex) {
      case 0: mmValue = max(0, mmValue - 0.1); break;  // Decrementa di 0.1mm
      case 1: microstepIndex = (microstepIndex - 1 + 5) % 5; break;  // Scorri tra i microstep
      case 2: velocita = max(0, velocita - 10); break;  // Decrementa velocità
      case 3: accelerazione = max(0, accelerazione - 10); break;  // Decrementa accelerazione
      case 4: pezziTarget = max(0, pezziTarget - 1); break;  // Decrementa il contatore
    }
    delay(100);
  }

  // Uscita dal menu
  if (buttonPressed(btn1) ) {
      inMenu = false;
    delay(500);  // Debounce
  }
}

void displaySettings() {
  lcd.clear();
  lcd.print("Lunghezza: ");
  lcd.print(mmValue, 1);
  lcd.setCursor(0, 1);
  lcd.print("Pezzi: ");
  lcd.print(contatorePezzi);
  lcd.print("/");
  lcd.print(pezziTarget);
}

void eseguiAvanzamento() {
  if (contatorePezzi < pezziTarget) {
    setMicrostep(mmValue);  // Imposta automaticamente il microstep in base agli mm
    stepper.setMaxSpeed(velocitaToSteps(velocita));  // Imposta velocità
    stepper.setAcceleration(accelerazioneToSteps(accelerazione));  // Imposta accelerazione
    stepper.move(mmToSteps(mmValue));  // Converte mm in step
    stepper.runToPosition();
    contatorePezzi++;
  } else {
    lcd.clear();
    lcd.print("Target raggiunto");
  }
}

void setMicrostep(float mm) {
  // Imposta il microstep migliore in base al valore degli mm da avanzare
  if (mm > 5) {
    microstepIndex = 0;  // Full-step
  } else if (mm > 2) {
    microstepIndex = 1;  // Half-step
  } else if (mm > 1) {
    microstepIndex = 2;  // 1/4-step
  } else if (mm > 0.5) {
    microstepIndex = 3;  // 1/8-step
  } else {
    microstepIndex = 4;  // 1/16-step
  }

  // Applica il microstep
  digitalWrite(ms1, msSettings[microstepIndex][0]);
  digitalWrite(ms2, msSettings[microstepIndex][1]);
  digitalWrite(ms3, msSettings[microstepIndex][2]);
}

long mmToSteps(float mm) {
  // Conversione da mm a step basata sul diametro della ruota
  const int stepsPerRevolution = 200;  // Numero di step per giro a full-step
  return (long)((mm / CIRCUMFERENCE) * stepsPerRevolution * pow(2, microstepIndex));
}

float velocitaToSteps(float vel) {
  // Converte la velocità in mm/s in steps/s
  const int stepsPerRevolution = 200;
  return (vel / CIRCUMFERENCE) * stepsPerRevolution * pow(2, microstepIndex);
}

float accelerazioneToSteps(float acc) {
  // Converte l'accelerazione in mm/s² in steps/s²
  const int stepsPerRevolution = 200;
  return (acc / CIRCUMFERENCE) * stepsPerRevolution * pow(2, microstepIndex);
}