#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Encoder.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

// Encoder and Button Pins
#define ENCODER_PIN_A 3
#define ENCODER_PIN_B 2
#define ENCODER_BUTTON A0
Encoder encoder(ENCODER_PIN_A, ENCODER_PIN_B);

int cursor_position = -1; // Cursor for post-movement menu
float mm_position = 0.0;

// State Variables
enum State { MAIN_MENU, ZERO_CALIBRATION, FENCE_TOOL_MENU, SELECT_MM, MOVEMENT, POST_MOVEMENT };
State currentState = MAIN_MENU;

bool isFenceSelected = true; // Default selection


void displayMainMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Welcome!");
  lcd.setCursor(0, 1);
  lcd.print(cursor_position == 0 ? "> Zero Calibration" : "  Zero Calibration");
  lcd.setCursor(0, 2);
  lcd.print(cursor_position == 1 ? "> Fence & Tool" : "  Fence & Tool");
  lcd.setCursor(0, 3);
  lcd.print(cursor_position == 2 ? "> Apps" : "  Apps");
}


void displayCalibrationMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Zero Calibration");
  lcd.setCursor(0, 3);
  lcd.print(cursor_position == 3 ? "> Back to Menu" : "  Back to Menu");
}

void displayFenceToolMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Fence & Tool Menu");
  lcd.setCursor(0, 1);
  lcd.print(isFenceSelected ? "> Fence" : "  Fence");
  lcd.setCursor(0, 2);
  lcd.print(isFenceSelected ? "  Tool" : "> Tool");
  lcd.setCursor(0, 3);
  lcd.print(cursor_position == 3 ? "> Back to Menu" : "  Back to Menu");
}

void displayMMSelection(const char* mode) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Set MM - Mode:");
  lcd.print(mode);
  lcd.setCursor(0, 1);
  lcd.print("Value: ");
  lcd.print(mm_position);
  lcd.print(" mm");
  lcd.setCursor(0, 2);
  lcd.print("Press to Confirm");
}

void updateDisplay() {
  static int last_state = -1;
  static int last_cursor = -1;
  if (last_state != currentState || last_cursor != cursor_position) {
    last_state = currentState;
    last_cursor = cursor_position;
    switch (currentState) {
      case MAIN_MENU:
        displayMainMenu();
        break;

      case ZERO_CALIBRATION:
        displayCalibrationMenu();
        break;

      case FENCE_TOOL_MENU:
        displayFenceToolMenu();
        break;

      case SELECT_MM:
        displayMMSelection("Mode A");
        break;
    }
  }
}

void setup() {
  pinMode(ENCODER_BUTTON, INPUT_PULLUP);
  Serial.begin(115200);

  encoder.write(-1);  // Cosi darà 0 come risultato per la prima rotazione
  lcd.init();
  lcd.backlight();
  displayMainMenu();
}

void loop() {
  updateDisplay();

  static long previous_encoder_position = -1;
  unsigned long encoder_position = encoder.read() >> 2;
  if (encoder_position != previous_encoder_position) {
    previous_encoder_position = encoder_position;
    cursor_position = (encoder_position) % 4;
    Serial.print("Cursor position ");
    Serial.println(cursor_position);
  }

  bool button_pressed = digitalRead(ENCODER_BUTTON) == LOW;

  // Se il ritorno al main menu lo metti sempre nella stessa posizione
  // semplifichi di molto la gestione (altrimenti devi fare un if per ciascuno stato)
  if (button_pressed && cursor_position == 3) {
    currentState = MAIN_MENU;
  }

  switch (currentState) {
    case MAIN_MENU:
      if (button_pressed) {

        if (cursor_position == 0) currentState = ZERO_CALIBRATION;
        else if (cursor_position == 1) currentState = FENCE_TOOL_MENU;
        // etc etc etc (forse sarebbe meglio usare uno switch/case anche qui)
        updateDisplay();
        delay(300);
      }
      break;

    case ZERO_CALIBRATION:
      if (button_pressed) {
        updateDisplay();
        delay(300);
      }
      break;

    case FENCE_TOOL_MENU:
      if (button_pressed) {
        currentState = SELECT_MM;
        updateDisplay();
        delay(300);
      }
      break;
  }
}