#include <LiquidCrystal.h>

#define BTN_UP    32
#define BTN_DOWN  33
#define BTN_OK    34
#define BTN_BACK  35
#define led       15
#define lazer     25
#define driver_ON_OFF     23
// Stepper & limit switch pins
#define STEP_X 22
#define DIR_X  21
#define STEP_Y 19
#define DIR_Y  18
#define LIM_X  17
#define LIM_Y  16

int lazer_power=0;

LiquidCrystal lcd(5, 4, 0, 2, 14, 12);

const char* menu[] = {
  "Pusk GCode",
  "Manual XY",
  "Homing XY",
  "Set Laser",
  "Laser Power",
  "Info"
};
size_t menu_len = sizeof(menu)/sizeof(menu[0]);
int menu_pos = 0;
// Переменные для отслеживания текущего положения
long currentX = 0;
long currentY = 0;

void waitButtonReleaseAll() {
  while (
    digitalRead(BTN_UP) == HIGH ||
    digitalRead(BTN_DOWN) == HIGH ||
    digitalRead(BTN_OK) == HIGH ||
    digitalRead(BTN_BACK) == HIGH ) delay(20);
}

// Функция для шагающего мотора с сохранением шагов
void stepMotor(int stepPin, int dirPin, int dir, int steps, int us=400) {
  digitalWrite(dirPin, dir > 0 ? HIGH : LOW);
  for (int i=0; i<steps; i++) {
    digitalWrite(stepPin, HIGH); delayMicroseconds(us);
    digitalWrite(stepPin, LOW);  delayMicroseconds(us);
    
    // Сохраняем изменение позиции
    if (dirPin == DIR_X) {
      currentX += dir * steps / steps;
    } else if (dirPin == DIR_Y) {
      currentY += dir * steps / steps;
    }
  }
}

// HOMING — ищет оба лимита X,Y (концевики)
void homeXY() {
  lcd.clear();
  lcd.print("Homing...");
  delay(200);

  // X axis
  digitalWrite(DIR_X, LOW); // Идем в отрицательном направлении
  while(digitalRead(LIM_X)==HIGH) { // Ждем достижения лимита
    stepMotor(STEP_X, DIR_X, -1, 1, 400);
    delay(1);
  }
  currentX = 0; // Сброс шагов при достижении начала
  delay(200);

  // Y axis
  digitalWrite(DIR_Y, LOW);
  while(digitalRead(LIM_Y)==HIGH) {
    stepMotor(STEP_Y, DIR_Y, -1, 1, 400);
    delay(1);
  }
  currentY = 0; // Сброс шагов при достижении начала
  lcd.clear();
  lcd.print("XY Home OK");
  delay(800);
}

// Ручное управление с показом текущих координат
void manualXY() {
  lcd.clear();
  lcd.setCursor(0,1);
  lcd.print("<-Y  OK  Y->");
  delay(250);
  bool inXY=true;
  while (inXY) {
    // Показываем текущие координаты
    lcd.setCursor(0,0);
    lcd.print("X:"); lcd.print(currentX); lcd.print(", Y:"); lcd.print(currentY);lcd.print(" ");

    // Обработка кнопок управления
    if (digitalRead(BTN_UP)==HIGH) { // Движение вверх по оси X (+)
      unsigned long t0 = millis();
      int steps = 1;
      while(digitalRead(BTN_UP)==HIGH) {
        if (millis()-t0>600) steps=100;
        stepMotor(STEP_X, DIR_X, +1, steps, 350);
        lcd.setCursor(0,1);
        lcd.print("Move X+    ");
        delay(steps>1?70:200);
      }
      lcd.setCursor(0,1); lcd.print("            ");
    }
    else if (digitalRead(BTN_DOWN)==HIGH) { // Движение вниз по оси X (-)
      unsigned long t0 = millis();
      int steps = 1;
      while(digitalRead(BTN_DOWN)==HIGH) {
        if (millis()-t0>600) steps=100;
        stepMotor(STEP_X, DIR_X, -1, steps, 350);
        lcd.setCursor(0,1);
        lcd.print("Move X-    ");
        delay(steps>1?70:200);
      }
      lcd.setCursor(0,1); lcd.print("            ");
    }
    else if (digitalRead(BTN_OK)==HIGH) { // Движение вперед по оси Y (+)
      unsigned long t0 = millis();
      int steps = 1;
      while(digitalRead(BTN_OK)==HIGH) {
        if (millis()-t0>600) steps=100;
        stepMotor(STEP_Y, DIR_Y, +1, steps, 350);
        lcd.setCursor(0,1);
        lcd.print("Move Y+    ");
        delay(steps>1?70:200);
      }
      lcd.setCursor(0,1); lcd.print("            ");
    }
    else if (digitalRead(BTN_BACK)==HIGH) { // Движение назад по оси Y (-)
      unsigned long t0 = millis();
      int steps = 1;
      while(digitalRead(BTN_BACK)==HIGH) {
        if (millis()-t0>600) steps=100;
        stepMotor(STEP_Y, DIR_Y, -1, steps, 350);
        lcd.setCursor(0,1);
        lcd.print("Move Y-    ");
        delay(steps>1?70:200);
      }
      lcd.setCursor(0,1); lcd.print("            ");
    }
    // Выход из режима при совместном нажатии OK+BACK
    if (digitalRead(BTN_OK)==HIGH && digitalRead(BTN_BACK)==HIGH) {
      inXY=false;
      delay(400);
    }
  }
}
void adjustLaserPower() {
  showLaserPower();
  bool inSubMenu = true;
  while (inSubMenu) {
    if (digitalRead(BTN_UP) == HIGH) {
      if (lazer_power<1023)lazer_power+=10;
      if (lazer_power>1023)lazer_power=1022;
      analogWrite(lazer, lazer_power);
      showLaserPower();
      delay(80);
    }
    if (digitalRead(BTN_DOWN) == HIGH) {
      if (lazer_power>0)lazer_power-=10;
      if (lazer_power<=0)lazer_power=0;
      analogWrite(lazer, lazer_power);
      showLaserPower();
      delay(80);
    }
    if (digitalRead(BTN_OK) == HIGH || digitalRead(BTN_BACK) == HIGH) {
      inSubMenu = false;
      showMenu();
      delay(400);
    }
  }
}
void showLaserPower() {
  lcd.clear();
  lcd.print("Lzr Power:");
  lcd.setCursor(0,1);
  lcd.print(lazer_power);
  lcd.print("%");
}
void showMenu() {
  lcd.clear();
  for (int i=0; i<2 && (menu_pos+i)<menu_len; i++) {
    lcd.setCursor(0,i);
    if (i==0) lcd.print(">");
    else lcd.print(" ");
    lcd.print(menu[(menu_pos+i)%menu_len]);
  }
}
void setup() {
  pinMode(lazer, OUTPUT);
  analogWrite(lazer, lazer_power);
  lcd.begin(16,2);
  lcd.print("Privet!");

  pinMode(BTN_UP, INPUT_PULLDOWN);
  pinMode(BTN_DOWN, INPUT_PULLDOWN);
  pinMode(BTN_OK, INPUT_PULLDOWN);
  pinMode(BTN_BACK, INPUT_PULLDOWN);
  pinMode(led, OUTPUT);
  // Stepper pins
  pinMode(STEP_X, OUTPUT); pinMode(DIR_X, OUTPUT);
  pinMode(STEP_Y, OUTPUT); pinMode(DIR_Y, OUTPUT);
  pinMode(LIM_X, INPUT_PULLUP); pinMode(LIM_Y, INPUT_PULLUP);

  delay(1500);
  showMenu();
}

void loop() {
  if (digitalRead(BTN_UP) == HIGH) {
    digitalWrite(led, HIGH);
    menu_pos = (menu_pos - 1 + menu_len) % menu_len;
    showMenu();
    waitButtonReleaseAll();
    delay(200);
    digitalWrite(led, LOW);
  }
  else if (digitalRead(BTN_DOWN) == HIGH) {
    digitalWrite(led, HIGH);
    menu_pos = (menu_pos + 1) % menu_len;
    showMenu();
    waitButtonReleaseAll();
    delay(200);
    digitalWrite(led, LOW);
  }
  else if (digitalRead(BTN_OK) == HIGH) {
    digitalWrite(led, HIGH);
    lcd.clear();
    lcd.print("Vibrano:");
    lcd.setCursor(0,1);
    lcd.print(menu[menu_pos]);
    delay(200);
    waitButtonReleaseAll();

    if (menu_pos == 1) manualXY(); // Manual XY
    if (menu_pos == 2) homeXY();   // Homing XY
    if (menu_pos == 4) adjustLaserPower();

    showMenu();
    digitalWrite(led, LOW);
  }
}