#include <Arduino.h>

// ====================== ПИНЫ ======================
// Управление
#define PIN_BOOT          0   // Кнопка BOOT (на плате), активна при LOW
#define PIN_EXT_TRIG      7   // Внешний триггер, активен по HIGH

// Крышка (дрон-порт)
#define LID_STEP_PIN      1   // STEP драйвера крышки
#define LID_DIR_PIN       2   // DIR драйвера крышки
#define LID_CLOSED_PIN    3   // Концевик "крышка закрыта" (LOW = закрыта)

// Лифт
#define LIFT_STEP_PIN     4   // STEP для обоих драйверов лифта
#define LIFT_DIR_PIN      5   // DIR для обоих драйверов лифта
#define LIFT_BOTTOM_PIN   6   // Концевик "лифт внизу" (LOW = внизу)

// ====================== НАСТРОЙКИ ДВИЖЕНИЯ ======================

const int  STEPS_PER_MM        = 100;      // шагов на мм (для крышки и лифта одинаково)

// Лифт
const int  LIFT_TRAVEL_MM      = 200;      // ход лифта вверх = 20 см
const long LIFT_TRAVEL_STEPS   = (long)LIFT_TRAVEL_MM * STEPS_PER_MM;
const long LIFT_MAX_DOWN_STEPS = LIFT_TRAVEL_STEPS * 1.1;   // лимит на опускание

// Крышка
const int  LID_OPEN_MM         = 500;      // открыть крышку на 50 см
const long LID_OPEN_STEPS      = (long)LID_OPEN_MM * STEPS_PER_MM;
const long LID_MAX_CLOSE_STEPS = LID_OPEN_STEPS * 1.1;      // лимит на закрытие

// Общие временные параметры шагов
const int  LIFT_SPEED_SPS      = 1500;     // скорость ~1500 шаг/сек
const int  STEP_INTERVAL_US    = 1000000 / LIFT_SPEED_SPS;
const int  STEP_PULSE_US       = 3;

// Защита от дребезга и "двойного" клика
const unsigned long ABORT_ARM_DELAY_MS   = 150;   // после старта движения, через сколько мс разрешить стоп по BOOT
const unsigned long TRIGGER_DEBOUNCE_MS  = 200;   // дебаунс событий BOOT/PIN7

// Направления (можно поменять, если моторы крутятся "не туда")
const uint8_t LIFT_DIR_UP_LEVEL      = LOW;   // лифт вверх
const uint8_t LIFT_DIR_DOWN_LEVEL    = HIGH;  // лифт вниз
const uint8_t LID_DIR_OPEN_LEVEL     = LOW;   // крышка открывается
const uint8_t LID_DIR_CLOSE_LEVEL    = HIGH;  // крышка закрывается

// ====================== ГЛОБАЛЬНЫЕ ПЕРЕМЕННЫЕ ======================

unsigned long lastTriggerTime  = 0;
unsigned long lastStatusPrint  = 0;

long liftPositionSteps         = 0;    // 0 = лифт внизу (по концевику)
long lidPositionSteps          = 0;    // 0 = крышка закрыта (по концевику), >0 = открыта

bool lastBootPressed           = false;
bool lastExtActive             = false;

// ====================== ХЕЛПЕРЫ ======================

bool isBootPressed() {
  return (digitalRead(PIN_BOOT) == LOW);   // LOW = нажата
}

bool isExtActive() {
  return (digitalRead(PIN_EXT_TRIG) == HIGH);  // HIGH = активен
}

// Лифт внизу?
bool isLiftBottom() {
  return (digitalRead(LIFT_BOTTOM_PIN) == LOW);  // концевик на GND -> LOW = внизу
}

// Крышка закрыта?
bool isLidClosed() {
  return (digitalRead(LID_CLOSED_PIN) == LOW);   // концевик на GND -> LOW = закрыта
}

// Один шаг на заданный STEP-пин
void doStepOnce(uint8_t stepPin) {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(STEP_PULSE_US);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(STEP_INTERVAL_US - STEP_PULSE_US);
}

// ====================== ДВИЖЕНИЕ ЛИФТА ======================

// Лифт ВВЕРХ на фиксированные 20 см
// Возвращает true, если доехали до конца, false — если остановлено по BOOT.
bool moveLiftUpFixed() {
  if (isLidClosed()) {
    Serial.println("ОШИБКА: попытка поднять лифт при закрытой крышке — запрещено.");
    return false;
  }

  Serial.println("\n=== ЛИФТ: ДВИЖЕНИЕ ВВЕРХ (20 см) ===");
  Serial.printf("LIFT_TRAVEL_STEPS = %ld\n", LIFT_TRAVEL_STEPS);

  digitalWrite(LIFT_DIR_PIN, LIFT_DIR_UP_LEVEL);
  delayMicroseconds(10);

  const unsigned long moveStart = millis();
  bool aborted    = false;
  bool abortArmed = false;

  bool bootPrev = isBootPressed();

  for (long i = 0; i < LIFT_TRAVEL_STEPS; i++) {
    unsigned long now     = millis();
    bool          bootNow = isBootPressed();

    // 1) Ждём, пока:
    //    - пройдёт ABORT_ARM_DELAY_MS
    //    - и кнопка будет отпущена
    if (!abortArmed) {
      if ((now - moveStart) > ABORT_ARM_DELAY_MS && !bootNow) {
        abortArmed = true;
      }
    } else {
      // 2) После этого: фронт BOOT (отпущено -> нажато) = СТОП
      if (!bootPrev && bootNow) {
        aborted = true;
        break;
      }
    }

    bootPrev = bootNow;

    doStepOnce(LIFT_STEP_PIN);
    liftPositionSteps++;
  }

  if (aborted) {
    Serial.println("ЛИФТ: движение ВВЕРХ остановлено по BOOT");
    // Ждём отпускания кнопки, чтобы следующее нажатие было "новым"
    while (isBootPressed()) {
      delay(1);
    }
    return false;
  }

  Serial.println("ЛИФТ: подъём завершён (предположительно ВЕРХ)");
  return true;
}

// Лифт ВНИЗ до нижнего концевика
bool moveLiftDownToBottom() {
  Serial.println("\n=== ЛИФТ: ДВИЖЕНИЕ ВНИЗ до концевика ===");

  digitalWrite(LIFT_DIR_PIN, LIFT_DIR_DOWN_LEVEL);
  delayMicroseconds(10);

  const unsigned long moveStart = millis();
  long stepsDone     = 0;
  bool aborted       = false;
  bool abortArmed    = false;

  bool bootPrev = isBootPressed();

  while (!isLiftBottom() && stepsDone < LIFT_MAX_DOWN_STEPS) {
    unsigned long now     = millis();
    bool          bootNow = isBootPressed();

    if (!abortArmed) {
      if ((now - moveStart) > ABORT_ARM_DELAY_MS && !bootNow) {
        abortArmed = true;
      }
    } else {
      if (!bootPrev && bootNow) {
        aborted = true;
        break;
      }
    }

    bootPrev = bootNow;

    doStepOnce(LIFT_STEP_PIN);
    stepsDone++;

    if (liftPositionSteps > 0) {
      liftPositionSteps--;
    }
  }

  if (aborted) {
    Serial.printf("ЛИФТ: движение ВНИЗ остановлено по BOOT после %ld шагов\n", stepsDone);
    while (isBootPressed()) {
      delay(1);
    }
    return false;
  }

  if (isLiftBottom()) {
    Serial.printf("ЛИФТ: достигнут нижний концевик за %ld шагов\n", stepsDone);
    liftPositionSteps = 0;
    return true;
  } else {
    Serial.printf("ЛИФТ: ВНИМАНИЕ — нижний концевик не сработал, достигнут лимит %ld шагов!\n", stepsDone);
    return false;
  }
}

// ====================== ДВИЖЕНИЕ КРЫШКИ ======================

// Крышка ОТКРЫТЬ на 50 см (по шагам)
bool moveLidOpenFixed() {
  Serial.println("\n=== КРЫШКА: ОТКРЫТИЕ на 50 см ===");
  Serial.printf("LID_OPEN_STEPS = %ld\n", LID_OPEN_STEPS);

  digitalWrite(LID_DIR_PIN, LID_DIR_OPEN_LEVEL);
  delayMicroseconds(10);

  const unsigned long moveStart = millis();
  bool aborted    = false;
  bool abortArmed = false;

  bool bootPrev = isBootPressed();

  for (long i = 0; i < LID_OPEN_STEPS; i++) {
    unsigned long now     = millis();
    bool          bootNow = isBootPressed();

    if (!abortArmed) {
      if ((now - moveStart) > ABORT_ARM_DELAY_MS && !bootNow) {
        abortArmed = true;
      }
    } else {
      if (!bootPrev && bootNow) {
        aborted = true;
        break;
      }
    }

    bootPrev = bootNow;

    doStepOnce(LID_STEP_PIN);
    lidPositionSteps++;
  }

  if (aborted) {
    Serial.println("КРЫШКА: открытие остановлено по BOOT");
    while (isBootPressed()) {
      delay(1);
    }
    return false;
  }

  Serial.println("КРЫШКА: открытие завершено (номинально 50 см)");
  return true;
}

// Крышка ЗАКРЫТЬ до концевика (можно только когда лифт внизу)
bool moveLidCloseToLimit() {
  if (!isLiftBottom()) {
    Serial.println("ОШИБКА: попытка закрыть крышку при НЕ опущенном лифте — запрещено.");
    return false;
  }

  Serial.println("\n=== КРЫШКА: ЗАКРЫТИЕ до концевика ===");

  digitalWrite(LID_DIR_PIN, LID_DIR_CLOSE_LEVEL);
  delayMicroseconds(10);

  const unsigned long moveStart = millis();
  long stepsDone  = 0;
  bool aborted    = false;
  bool abortArmed = false;

  bool bootPrev = isBootPressed();

  while (!isLidClosed() && stepsDone < LID_MAX_CLOSE_STEPS) {
    unsigned long now     = millis();
    bool          bootNow = isBootPressed();

    if (!abortArmed) {
      if ((now - moveStart) > ABORT_ARM_DELAY_MS && !bootNow) {
        abortArmed = true;
      }
    } else {
      if (!bootPrev && bootNow) {
        aborted = true;
        break;
      }
    }

    bootPrev = bootNow;

    doStepOnce(LID_STEP_PIN);
    stepsDone++;

    if (lidPositionSteps > 0) {
      lidPositionSteps--;
    }
  }

  if (aborted) {
    Serial.printf("КРЫШКА: закрытие остановлено по BOOT после %ld шагов\n", stepsDone);
    while (isBootPressed()) {
      delay(1);
    }
    return false;
  }

  if (isLidClosed()) {
    Serial.printf("КРЫШКА: закрыта, концевик сработал за %ld шагов\n", stepsDone);
    lidPositionSteps = 0;
    return true;
  } else {
    Serial.printf("КРЫШКА: ВНИМАНИЕ — концевик не сработал, достигнут лимит %ld шагов!\n", stepsDone);
    return false;
  }
}

// ====================== СЕКВЕНЦИИ ======================

// 1-я активация: из состояния "крышка закрыта, лифт внизу"
//   -> открыть крышку на 50 см
//   -> затем поднять лифт на 20 см
void sequenceOpenLidThenLiftUp() {
  Serial.println("\n=== СЕКВЕНЦИЯ 1: ОТКРЫТЬ крышку, затем ПОДНЯТЬ лифт ===");

  // На всякий случай доводим лифт в самый низ (если вдруг не внизу)
  if (!isLiftBottom()) {
    Serial.println("Лифт неожиданно не внизу, опускаю до концевика...");
    if (!moveLiftDownToBottom()) {
      Serial.println("Прерывание при опускании лифта, выходим из последовательности.");
      return;
    }
  }

  // Если крышка почему-то не закрыта — закрываем
  if (!isLidClosed()) {
    Serial.println("Крышка не закрыта, закрываю до концевика...");
    if (!moveLidCloseToLimit()) {
      Serial.println("Прерывание при закрытии крышки, выходим из последовательности.");
      return;
    }
  }

  // 1) ОТКРЫВАЕМ крышку на 50 см
  if (!moveLidOpenFixed()) {
    Serial.println("Открытие крышки прервано, лифт не поднимаем.");
    return;
  }

  // 2) ПОДНИМАЕМ лифт на 20 см (если крышка уже не закрыта)
  if (isLidClosed()) {
    Serial.println("ОШИБКА: после открытия крышка почему-то считается закрытой, лифт не поднимаю.");
    return;
  }

  moveLiftUpFixed();
}

// 2-я активация: из состояния "крышка открыта, лифт вверху"
//   -> опускаем лифт до концевика
//   -> затем закрываем крышку
void sequenceLiftDownThenCloseLid() {
  Serial.println("\n=== СЕКВЕНЦИЯ 2: ОПУСТИТЬ лифт, затем ЗАКРЫТЬ крышку ===");

  // 1) ОПУСКАЕМ лифт
  if (!moveLiftDownToBottom()) {
    Serial.println("Прерывание при опускании лифта, крышку не закрываем.");
    return;
  }

  if (!isLiftBottom()) {
    Serial.println("ОШИБКА: лифт не дошёл до нижнего концевика, крышку закрывать нельзя.");
    return;
  }

  // 2) ЗАКРЫВАЕМ крышку (интерлок уже проверен внутри moveLidCloseToLimit)
  if (!isLidClosed()) {
    moveLidCloseToLimit();
  } else {
    Serial.println("Крышка уже закрыта, ничего не делаем.");
  }
}

// ====================== ОБРАБОТКА АКТИВАЦИИ ======================

void handleActivation() {
  bool liftBottom = isLiftBottom();
  bool lidClosed  = isLidClosed();

  Serial.printf("\n[АКТИВАЦИЯ] liftBottom=%s, lidClosed=%s\n",
                liftBottom ? "TRUE" : "FALSE",
                lidClosed  ? "TRUE" : "FALSE");

  // Нормальный цикл:
  // 1) старт: лифт внизу, крышка закрыта
  // 2) после первой активации: лифт вверх, крышка открыта
  // 3) после второй активации: снова лифт внизу, крышка закрыта

  if (liftBottom && lidClosed) {
    // База: всё закрыто, лифт внизу -> открываем крышку и поднимаем лифт
    sequenceOpenLidThenLiftUp();
  }
  else if (!liftBottom && !lidClosed) {
    // Оба не на концевиках -> это "состояние полёта": лифт наверху, крышка открыта
    // Следующая активация: опустить лифт и закрыть крышку
    sequenceLiftDownThenCloseLid();
  }
  else if (liftBottom && !lidClosed) {
    // Лифт внизу, крышка открыта -> просто закрываем крышку
    Serial.println("Состояние: лифт внизу, крышка открыта -> закрываю крышку.");
    moveLidCloseToLimit();
  }
  else { // !liftBottom && lidClosed
    // Крышка закрыта, лифт не внизу (странное состояние) -> безопасно опускаем лифт
    Serial.println("Состояние: крышка закрыта, лифт не внизу -> опускаю лифт до концевика.");
    moveLiftDownToBottom();
  }
}

// ====================== SETUP ======================

void setup() {
  Serial.begin(115200);
  delay(500);

  Serial.println("\n\n=====================================");
  Serial.println("   ДВУХОСЕВАЯ СИСТЕМА: КРЫШКА + ЛИФТ");
  Serial.println("   Крышка: STEP=1, DIR=2, LIMIT(закрыто)=3 (LOW=закрыто)");
  Serial.println("   Лифт:   STEP=4, DIR=5, LIMIT(низ)=6 (LOW=низ)");
  Serial.println("   Управление: BOOT (0, LOW), EXT (7, HIGH)");
  Serial.println("=====================================\n");

  pinMode(PIN_BOOT,       INPUT_PULLUP);
  pinMode(PIN_EXT_TRIG,   INPUT);

  pinMode(LID_STEP_PIN,   OUTPUT);
  pinMode(LID_DIR_PIN,    OUTPUT);
  pinMode(LID_CLOSED_PIN, INPUT_PULLUP); // концевик на GND

  pinMode(LIFT_STEP_PIN,  OUTPUT);
  pinMode(LIFT_DIR_PIN,   OUTPUT);
  pinMode(LIFT_BOTTOM_PIN,INPUT_PULLUP); // концевик на GND

  digitalWrite(LID_STEP_PIN,  LOW);
  digitalWrite(LID_DIR_PIN,   LOW);
  digitalWrite(LIFT_STEP_PIN, LOW);
  digitalWrite(LIFT_DIR_PIN,  LOW);

  lastBootPressed = isBootPressed();
  lastExtActive   = isExtActive();

  Serial.printf("Старт: лифт внизу? %s, крышка закрыта? %s\n",
                isLiftBottom() ? "ДА" : "НЕТ",
                isLidClosed()  ? "ДА" : "НЕТ");

  // === ЛОГИКА ПРИ ВКЛЮЧЕНИИ ===
  // Сначала: если лифт НЕ внизу -> опускаем лифт.
  if (!isLiftBottom()) {
    Serial.println("Инициализация: лифт НЕ внизу, опускаю до концевика...");
    moveLiftDownToBottom();
  }

  // После этого, если лифт внизу и крышка НЕ закрыта -> закрываем крышку.
  if (isLiftBottom() && !isLidClosed()) {
    Serial.println("Инициализация: крышка НЕ закрыта, закрываю...");
    moveLidCloseToLimit();
  }

  Serial.printf("После инициализации: лифт внизу? %s, крышка закрыта? %s\n",
                isLiftBottom() ? "ДА" : "НЕТ",
                isLidClosed()  ? "ДА" : "НЕТ");
  Serial.println("\nГотово. Жду BOOT или PIN 7 для последовательности.");
}

// ====================== LOOP ======================

void loop() {
  unsigned long now = millis();

  bool bootNow = isBootPressed();
  bool extNow  = isExtActive();

  bool triggerEvent = false;

  // Фронт BOOT
  if (bootNow && !lastBootPressed && (now - lastTriggerTime > TRIGGER_DEBOUNCE_MS)) {
    triggerEvent = true;
    lastTriggerTime = now;
    Serial.println("\n[ТРЕГГЕР] BOOT нажат");
  }

  // Фронт внешнего триггера
  if (extNow && !lastExtActive && (now - lastTriggerTime > TRIGGER_DEBOUNCE_MS)) {
    triggerEvent = true;
    lastTriggerTime = now;
    Serial.println("\n[ТРЕГГЕР] Внешний вход PIN 7 = HIGH");
  }

  lastBootPressed = bootNow;
  lastExtActive   = extNow;

  // Обработка активации (когда ничего не крутится — все движения внутри блокирующие)
  if (triggerEvent) {
    handleActivation();
  }

  // Периодический статус
  if (now - lastStatusPrint > 1000) {
    lastStatusPrint = now;

    long liftMM = liftPositionSteps / STEPS_PER_MM;
    long lidMM  = lidPositionSteps  / STEPS_PER_MM;

    Serial.printf("СТАТУС: лифт ~%ld мм | крышка ~%ld мм | liftBottom=%s | lidClosed=%s\n",
                  liftMM, lidMM,
                  isLiftBottom() ? "YES" : "NO",
                  isLidClosed()  ? "YES" : "NO");
  }
}