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

// Підключення I2C LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Піни для енкодера
#define ENCODER_CLK 11
#define ENCODER_DT 12
#define ENCODER_SW 13

// Піни для крокових двигунів
#define STEP_PIN_X 8
#define DIR_PIN_X 7
#define STEP_PIN_Y 5
#define DIR_PIN_Y 6
#define ENABLE_PIN_X 9
#define ENABLE_PIN_Y 3

AccelStepper stepperX(AccelStepper::DRIVER, STEP_PIN_X, DIR_PIN_X);
AccelStepper stepperY(AccelStepper::DRIVER, STEP_PIN_Y, DIR_PIN_Y);

// Змінні для роботи меню
int menuIndex = 0;
int lastStateCLK;
int currentStateCLK;
int buttonState;
unsigned long lastButtonPress = 0;

// Структура меню
const char* menu[] = {"Move", "Settings", "Disable steppers"};
const char* moveMenu[] = {"Start", "Stop", "Back"};
const char* settingsMenu[] = {"Stepper X", "Stepper Y", "Back"};
const char* stepperMenu[] = {"Direction", "Speed", "Steps", "Back"};
const char* disableMenu[] = {"Disable X", "Disable Y", "Back"};

int currentMenuLevel = 0; // 0 - головне меню, 1 - підменю, і т.д.
const char** currentMenu = menu;
int menuSize = 3;

void setup() {
  lcd.init();
  lcd.backlight();
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);

  pinMode(ENABLE_PIN_X, OUTPUT);
  pinMode(ENABLE_PIN_Y, OUTPUT);
  digitalWrite(ENABLE_PIN_X, LOW); // Увімкнути драйвер X
  digitalWrite(ENABLE_PIN_Y, LOW); // Увімкнути драйвер Y

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

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

  lastStateCLK = digitalRead(ENCODER_CLK);

  lcd.setCursor(0, 0);
  lcd.print("Welcome!");
  delay(2000);
  lcd.clear();
  displayMenu();
}

void loop() {
  // Читання стану енкодера
  currentStateCLK = digitalRead(ENCODER_CLK);

  // Якщо стан змінився
  if (currentStateCLK != lastStateCLK && currentStateCLK == LOW) {
    if (digitalRead(ENCODER_DT) != currentStateCLK) {
      menuIndex++;
    } else {
      menuIndex--;
    }

    // Обмеження індексу меню
    menuIndex = constrain(menuIndex, 0, menuSize - 1);
    displayMenu();
  }

  lastStateCLK = currentStateCLK;

  // Читання кнопки енкодера
  buttonState = digitalRead(ENCODER_SW);

  if (buttonState == LOW) {
    if (millis() - lastButtonPress > 300) {
      lastButtonPress = millis();

      // Обробка вибору меню
      handleMenuSelection();
    }
  }

  stepperX.run();
  stepperY.run();
}

void displayMenu() {
  lcd.clear();
  lcd.print(currentMenu[menuIndex]);
}

void handleMenuSelection() {
  if (currentMenuLevel == 0) {
    // Головне меню
    if (menuIndex == 0) {
      currentMenu = moveMenu;
      menuSize = 3;
      currentMenuLevel++;
    } else if (menuIndex == 1) {
      currentMenu = settingsMenu;
      menuSize = 3;
      currentMenuLevel++;
    } else if (menuIndex == 2) {
      currentMenu = disableMenu;
      menuSize = 3;
      currentMenuLevel++;
    }
  } else {
    // Підменю
    if (strcmp(currentMenu[menuIndex], "Back") == 0) {
      if (currentMenu == moveMenu || currentMenu == settingsMenu || currentMenu == disableMenu) {
        currentMenu = menu;
        menuSize = 3;
        currentMenuLevel--;
      } else if (currentMenu == stepperMenu) {
        currentMenu = settingsMenu;
        menuSize = 3;
        currentMenuLevel--;
      }
    } else {
      // Виконання дій в підменю
      if (currentMenu == moveMenu) {
        executeMoveAction(menuIndex);
      } else if (currentMenu == settingsMenu) {
        currentMenu = stepperMenu;
        menuSize = 4;
        currentMenuLevel++;
      } else if (currentMenu == disableMenu) {
        executeDisableAction(menuIndex);
      }
    }
  }
  displayMenu();
}

void executeMoveAction(int action) {
  lcd.clear();
  if (action == 0) {
    lcd.print("Start Moving");
    stepperX.move(1000);
    stepperY.move(1000);
  } else if (action == 1) {
    lcd.print("Stopped");
    stepperX.stop();
    stepperY.stop();
  }
  delay(2000);
  lcd.clear();
}

void executeDisableAction(int action) {
  lcd.clear();
  if (action == 0) {
    lcd.print("X Disabled");
    digitalWrite(ENABLE_PIN_X, HIGH); // Вимкнути драйвер X
  } else if (action == 1) {
    lcd.print("Y Disabled");
    digitalWrite(ENABLE_PIN_Y, HIGH); // Вимкнути драйвер Y
  }
  delay(2000);
  lcd.clear();
}