#include <Arduino.h>


// =========================================================
//                 МОДУЛЬ 1: КРЫШКА + ЛИФТ
// =========================================================

// --- Пины крышки ---
#define LID_STEP_PIN      1   // STEP драйвера крышки
#define LID_DIR_PIN       2   // DIR драйвера крышки
#define LID_CLOSED_PIN    3   // Концевик "крышка закрыта" замыкаем на минус

// --- Пины лифта ---
#define LIFT_STEP_PIN     4   // STEP для обоих драйверов лифта
#define LIFT_DIR_PIN      5   // DIR для обоих драйверов лифта
#define LIFT_BOTTOM_PIN   6   // Концевик "лифт внизу" замыкаем на минус

// ====================== ПИНЫ ЦЕНТРОВКИ ======================
// Центровка - нижняя ось
#define M1_STEP_PIN 36
#define M1_DIR_PIN  37
#define M2_STEP_PIN 38
#define M2_DIR_PIN  39
#define M3_STEP_PIN 40
#define M3_DIR_PIN  41
#define M4_STEP_PIN 7
#define M4_DIR_PIN  8

// Центровка - верхняя ось
#define M5_STEP_PIN 9
#define M5_DIR_PIN  10
#define M6_STEP_PIN 11
#define M6_DIR_PIN  12









#define PIN_BOOT 0   // Кнопка BOOT (на плате), активна при LOW

// Дребезг для верхнеуровневого запуска последовательностей
const unsigned long TRIGGER_DEBOUNCE_MS  = 200;
const unsigned long ABORT_ARM_DELAY_MS   = 150;



// --- Параметры движения лифта/крышки ---
const int  LIFT_LID_STEPS_PER_MM      = 100;      // шагов на мм
const int  LIFT_SPEED_SPS             = 1500;     // скорость ~1500 шаг/сек
const int  LIFT_LID_STEP_INTERVAL_US  = 1000000 / LIFT_SPEED_SPS;
const int  LIFT_LID_STEP_PULSE_US     = 3;

// Лифт: ход вверх 20 см
const int  LIFT_TRAVEL_MM             = 200;// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
const long LIFT_TRAVEL_STEPS          = (long)LIFT_TRAVEL_MM * LIFT_LID_STEPS_PER_MM;

// Крышка: открытие на 50 см
const int  LID_OPEN_MM                = 500;// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
const long LID_OPEN_STEPS             = (long)LID_OPEN_MM * LIFT_LID_STEPS_PER_MM;

//защита проаорота по концевикам
const bool REQUIRE_LIFT_BOTTOM_FOR_SAFE_CLOSE = false;

// --- Защита по лимиту шагов при поиске концевиков ---
// true  = защита включена (по умолчанию)
// false = защиты нет: едем до срабатывания концевика сколько угодно шагов
const bool USE_HARD_STEPS_LIMIT       = false;

// Допустимая погрешность (запас) по шагам, например 10%
// Т.е. вниз/закрытие допускаем до 110% от расчётного хода
const float HOMING_MARGIN             = 0.10f;

// Максимальное число шагов при поиске концевиков
const long LIFT_MAX_DOWN_STEPS        = (long)(LIFT_TRAVEL_STEPS * (1.0f + HOMING_MARGIN));
const long LID_MAX_CLOSE_STEPS        = (long)(LID_OPEN_STEPS   * (1.0f + HOMING_MARGIN));

// --- Направления (при необходимости можно инвертировать) ---
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;  // крышка закрывается

// --- Глобальные переменные для модуля крышки/лифта ---
long liftPositionSteps = 0;    // 0 = лифт внизу (по концевику)
long lidPositionSteps  = 0;    // 0 = крышка закрыта (по концевику)

// =========================================================
 //                 МОДУЛЬ 2: ЦЕНТРОВКА СТОЛА (6 моторов)
//                 (из centrrovka_ok_clean_v2.ino)
// =========================================================

// ====================== НАСТРОЙКИ РЕВЕРСА DIR ======================
static const bool M1_DIR_NORMAL = false;
static const bool M2_DIR_NORMAL = true;
static const bool M3_DIR_NORMAL = true;
static const bool M4_DIR_NORMAL = true;
static const bool M5_DIR_NORMAL = true;
static const bool M6_DIR_NORMAL = true;



// ====================== ПАРАМЕТРЫ ДВИЖЕНИЯ ЦЕНТРОВКИ ======================
static const int  STEPS_PER_MM     = 2;     // шагов на мм
static const int  BOTTOM_AXIS_MM   = 204;   // ход "к центру" для нижней оси, мм
static const int  TOP_AXIS_MM      = 190;   // ход "к центру" для верхней оси, мм

static const long BOTTOM_AXIS_STEPS = (long)BOTTOM_AXIS_MM * (long)STEPS_PER_MM;
static const long TOP_AXIS_STEPS    = (long)TOP_AXIS_MM    * (long)STEPS_PER_MM;

static const int  STEP_PULSE_US    = 3;     // длительность импульса STEP
static const int  CENTER_SPEED_SPS = 50;   // скорость, шаг/сек

#define SPS_TO_INTERVAL_US(sps) (1000000UL / (unsigned long)(sps))

// ====================== ПРОСТОЙ ШАГОВИК ======================
class SimpleStepper {
public:
  void begin(uint8_t stepPin, uint8_t dirPin, int sps, bool dirNormal = true) {
    _step = stepPin;
    _dir = dirPin;
    _dirNormal = dirNormal;

    pinMode(_step, OUTPUT);
    pinMode(_dir, OUTPUT);
    digitalWrite(_step, LOW);

    _pos = 0;
    _target = 0;
    _moving = false;
    _lastStepUs = micros();
    setSpeedSps(sps);
  }

  void setSpeedSps(int sps) {
    if (sps < 1) sps = 1;
    _intervalUs = SPS_TO_INTERVAL_US(sps);
  }

  void moveTo(long target) {
    _target = target;
    if (_target != _pos) {
      _moving = true;
      updateDir();
    }
  }

  void stopHold() {
    _moving = false;
  }

  bool isBusy() const {
    return _moving;
  }

  long position() const {
    return _pos;
  }

  void update() {
    if (!_moving) return;

    uint32_t now = micros();
    if ((uint32_t)(now - _lastStepUs) >= _intervalUs) {
      digitalWrite(_step, HIGH);
      delayMicroseconds(STEP_PULSE_US);
      digitalWrite(_step, LOW);
      _lastStepUs = now;

      int dir = (_target > _pos) ? 1 : -1;
      _pos += dir;
      if (_pos == _target) _moving = false;
    }
  }

private:
  void updateDir() {
    int dir = (_target > _pos) ? 1 : -1;
    bool dirHigh = _dirNormal ? (dir > 0) : (dir < 0);
    digitalWrite(_dir, dirHigh ? HIGH : LOW);
  }

  uint8_t _step = 0, _dir = 0;
  bool _dirNormal = true;
  long _pos = 0, _target = 0;
  bool _moving = false;
  uint32_t _lastStepUs = 0, _intervalUs = 2000;
};

// ====================== ГЛОБАЛЬНЫЕ ОБЪЕКТЫ ЦЕНТРОВКИ ======================
SimpleStepper stepM1, stepM2, stepM3, stepM4, stepM5, stepM6;

// ====================== СОСТОЯНИЯ ЦЕНТРОВКИ ======================
enum CenterState {
  WAIT_CENTER,    // ждём BOOT для центровки (в нуле)
  CENTERING,      // едем в центровку
  WAIT_DECENTER,  // ждём BOOT для расцентровки (из центра)
  DECENTERING     // едем обратно в 0
};

static CenterState state = WAIT_CENTER;

// ====================== УТИЛИТЫ ЦЕНТРОВКИ ======================
static void printHeader(const char* msg) {
  Serial.println("=====================================");
  Serial.printf("%s\n", msg);
  Serial.printf("t=%lu ms\n", (unsigned long)millis());
  Serial.println("=====================================");
}

static bool allMotorsIdle() {
  return !stepM1.isBusy() && !stepM2.isBusy() && !stepM3.isBusy() &&
         !stepM4.isBusy() && !stepM5.isBusy() && !stepM6.isBusy();
}

static void stopAllMotors() {
  stepM1.stopHold();
  stepM2.stopHold();
  stepM3.stopHold();
  stepM4.stopHold();
  stepM5.stopHold();
  stepM6.stopHold();
}

static void startCentering() {
  printHeader("СТАРТ: центровка (движение к центру)");
  Serial.printf("BOTTOM: %d mm => %ld steps\n", BOTTOM_AXIS_MM, BOTTOM_AXIS_STEPS);
  Serial.printf("TOP   : %d mm => %ld steps\n", TOP_AXIS_MM, TOP_AXIS_STEPS);

  stepM1.moveTo(-BOTTOM_AXIS_STEPS);
  stepM4.moveTo(-BOTTOM_AXIS_STEPS);

  stepM2.moveTo(+BOTTOM_AXIS_STEPS);
  stepM3.moveTo(+BOTTOM_AXIS_STEPS);

  stepM5.moveTo(-TOP_AXIS_STEPS);
  stepM6.moveTo(+TOP_AXIS_STEPS);

  state = CENTERING;
}

static void startDecentering() {
  printHeader("СТАРТ: расцентровка (возврат в 0)");

  stepM1.moveTo(0);
  stepM2.moveTo(0);
  stepM3.moveTo(0);
  stepM4.moveTo(0);
  stepM5.moveTo(0);
  stepM6.moveTo(0);

  state = DECENTERING;
}

// Дебаунс + детектор нажатия BOOT для центровки
static bool bootPressedCenter() {
  static bool lastRaw = HIGH;
  static bool stable = HIGH;
  static uint32_t lastChangeMs = 0;

  bool raw = digitalRead(PIN_BOOT);
  if (raw != lastRaw) {
    lastRaw = raw;
    lastChangeMs = millis();
  }

  if ((millis() - lastChangeMs) > 25 && raw != stable) {
    stable = raw;
    if (stable == LOW) return true;
  }
  return false;
}

// Инициализация модуля центровки
void centeringInit() {
  stepM1.begin(M1_STEP_PIN, M1_DIR_PIN, CENTER_SPEED_SPS, M1_DIR_NORMAL);
  stepM2.begin(M2_STEP_PIN, M2_DIR_PIN, CENTER_SPEED_SPS, M2_DIR_NORMAL);
  stepM3.begin(M3_STEP_PIN, M3_DIR_PIN, CENTER_SPEED_SPS, M3_DIR_NORMAL);
  stepM4.begin(M4_STEP_PIN, M4_DIR_PIN, CENTER_SPEED_SPS, M4_DIR_NORMAL);
  stepM5.begin(M5_STEP_PIN, M5_DIR_PIN, CENTER_SPEED_SPS, M5_DIR_NORMAL);
  stepM6.begin(M6_STEP_PIN, M6_DIR_PIN, CENTER_SPEED_SPS, M6_DIR_NORMAL);

  printHeader("ГОТОВО: центровка стола (6 моторов)");
  Serial.println("Режим по умолчанию: WAIT_CENTER (0-позиция)");
  state = WAIT_CENTER;
}

// Один шаг логики центровки
void centeringLoopStep() {
  // шаги моторов
  stepM1.update();
  stepM2.update();
  stepM3.update();
  stepM4.update();
  stepM5.update();
  stepM6.update();

  // обработка BOOT внутри центровки
  if (bootPressedCenter()) {
    switch (state) {
      case WAIT_CENTER:
        // ручная центровка из нуля (если когда-то понадобится)
        startCentering();
        break;

      case WAIT_DECENTER:
        // по ТВОЕМУ сценарию: это как раз второй BOOT -> расцентровка
        startDecentering();
        break;

      case CENTERING:
        printHeader("СТОП: центровка прервана кнопкой");
        stopAllMotors();
        Serial.println("Теперь нажмите BOOT для расцентровки (в 0)");
        state = WAIT_DECENTER;
        break;

      case DECENTERING:
        printHeader("СТОП: расцентровка прервана кнопкой");
        stopAllMotors();
        Serial.println("Теперь нажмите BOOT для центровки");
        state = WAIT_CENTER;
        break;
    }
  }

  // завершение движений
  if (state == CENTERING && allMotorsIdle()) {
    printHeader("ГОТОВО: стол в центре");
    Serial.println("Ждём BOOT для РАСЦЕНТРОВКИ и дальнейшего закрытия порта.");
    state = WAIT_DECENTER;
  }

  if (state == DECENTERING && allMotorsIdle()) {
    printHeader("ГОТОВО: стол расцентрован (0)");
    Serial.println("Ждём верхнеуровневую команду для закрытия порта.");
    state = WAIT_CENTER;
  }
}

// =========================================================
//          ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ ДЛЯ КРЫШКИ/ЛИФТА
// =========================================================

bool isLiftBottom() {
  return (digitalRead(LIFT_BOTTOM_PIN) == HIGH);
}

bool isLidClosed() {
  return (digitalRead(LID_CLOSED_PIN) == HIGH);
}

static void doLiftLidStep(uint8_t stepPin) {
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(LIFT_LID_STEP_PULSE_US);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(LIFT_LID_STEP_INTERVAL_US - LIFT_LID_STEP_PULSE_US);
}

// Лифт ВВЕРХ на 20 см
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 = (digitalRead(PIN_BOOT) == LOW);

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

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

    bootPrev = bootNow;

    doLiftLidStep(LIFT_STEP_PIN);
    liftPositionSteps++;
  }

  if (aborted) {
    Serial.println("ЛИФТ: движение ВВЕРХ остановлено по BOOT");
    while (digitalRead(PIN_BOOT) == LOW) {
      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 = (digitalRead(PIN_BOOT) == LOW);

  // если защита включена -> ограничиваем stepsDone по LIFT_MAX_DOWN_STEPS
  // если выключена -> шагов бесконечно много (пока не сработает концевик)
  long maxSteps = USE_HARD_STEPS_LIMIT ? LIFT_MAX_DOWN_STEPS : 2147483647L;

  while (!isLiftBottom() && stepsDone < maxSteps) {
    unsigned long now     = millis();
    bool          bootNow = (digitalRead(PIN_BOOT) == LOW);

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

    bootPrev = bootNow;

    doLiftLidStep(LIFT_STEP_PIN);
    stepsDone++;

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

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

  if (isLiftBottom()) {
    Serial.printf("ЛИФТ: достигнут нижний концевик за %ld шагов\n", stepsDone);
    liftPositionSteps = 0;
    return true;
  } else {
    if (USE_HARD_STEPS_LIMIT) {
      Serial.printf(
        "ЛИФТ: ВНИМАНИЕ — концевик не сработал, достигнут лимит %ld шагов (≈%.1f%% от расчётного хода)\n",
        stepsDone, 100.0f * (float)stepsDone / (float)LIFT_TRAVEL_STEPS
      );
      return false;
    } else {
      Serial.printf(
        "ЛИФТ: концевик НЕ сработал, достигнут шаговый лимит %ld, НО защита отключена — продолжаем как будто лифт внизу\n",
        stepsDone
      );
      // логически считаем, что лифт "внизу"
      liftPositionSteps = 0;
      return true;
    }
  }
}


// Крышка ОТКРЫТЬ на 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 = (digitalRead(PIN_BOOT) == LOW);

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

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

    bootPrev = bootNow;

    doLiftLidStep(LID_STEP_PIN);
    lidPositionSteps++;
  }

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

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

// Крышка ЗАКРЫТЬ до концевика
bool moveLidCloseToLimit() {
  // Дополнительная защита: закрывать крышку только если лифт внизу
  if (REQUIRE_LIFT_BOTTOM_FOR_SAFE_CLOSE && !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 = (digitalRead(PIN_BOOT) == LOW);

  // Если защита включена — ограничиваем шаги,
  // если выключена — считаем maxSteps очень большим числом
  long maxSteps = USE_HARD_STEPS_LIMIT ? LID_MAX_CLOSE_STEPS : 2147483647L;

  while (!isLidClosed() && stepsDone < maxSteps) {
    unsigned long now     = millis();
    bool          bootNow = (digitalRead(PIN_BOOT) == LOW);

    // Антидребезг: сначала ждём отпускания BOOT после старта,
    // потом реагируем только на новое нажатие
    if (!abortArmed) {
      if ((now - moveStart) > ABORT_ARM_DELAY_MS && !bootNow) {
        abortArmed = true;
      }
    } else {
      if (!bootPrev && bootNow) {
        aborted = true;
        break;
      }
    }

    bootPrev = bootNow;

    doLiftLidStep(LID_STEP_PIN);
    stepsDone++;

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

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

  if (isLidClosed()) {
    Serial.printf("КРЫШКА: закрыта, концевик сработал за %ld шагов\n", stepsDone);
    lidPositionSteps = 0;
    return true;
  } else {
    if (USE_HARD_STEPS_LIMIT) {
      Serial.printf(
        "КРЫШКА: ВНИМАНИЕ — концевик не сработал, достигнут лимит %ld шагов (≈%.1f%% от расчётного хода)\n",
        stepsDone, 100.0f * (float)stepsDone / (float)LID_OPEN_STEPS
      );
      return false;
    } else {
      Serial.printf(
        "КРЫШКА: концевик НЕ сработал, достигнут шаговый лимит %ld, НО защита отключена — считаем, что крышка закрыта\n",
        stepsDone
      );
      lidPositionSteps = 0;   // логически считаем её закрытой
      return true;
    }
  }
}



// =========================================================
//                ПОСЛЕДОВАТЕЛЬНОСТИ МОДУЛЯ 1
// =========================================================

// Открыть крышку и поднять лифт
bool sequenceOpenLidThenLiftUp() {
  Serial.println("\n=== СЕКВЕНЦИЯ: ОТКРЫТЬ крышку + ПОДНЯТЬ лифт ===");

  if (!isLiftBottom()) {
    Serial.println("Лифт не внизу, опускаю до концевика...");
    if (!moveLiftDownToBottom()) {
      Serial.println("Прервано при опускании лифта.");
      return false;
    }
  }

  if (!isLidClosed()) {
    Serial.println("Крышка не закрыта, закрываю...");
    if (!moveLidCloseToLimit()) {
      Serial.println("Прервано при закрытии крышки.");
      return false;
    }
  }

  if (!moveLidOpenFixed()) {
    Serial.println("Прервано при открытии крышки.");
    return false;
  }

  if (isLidClosed()) {
    Serial.println("ОШИБКА: крышка внезапно считается закрытой, лифт не поднимаю.");
    return false;
  }

  if (!moveLiftUpFixed()) {
    Serial.println("Прервано при подъёме лифта.");
    return false;
  }

  Serial.println("СЕКВЕНЦИЯ: крышка открыта, лифт поднят.");
  return true;
}

// Опустить лифт и закрыть крышку
bool sequenceLiftDownThenCloseLid() {
  Serial.println("\n=== СЕКВЕНЦИЯ: ОПУСТИТЬ лифт + ЗАКРЫТЬ крышку ===");

  // 1) Опускаем лифт до нижнего концевика (или до "условного низа", если лимит отключён)
  if (!moveLiftDownToBottom()) {
    Serial.println("Прерывание при опускании лифта, крышку не закрываем.");
    return false;
  }

  // Если безопасный режим включён, требуем реального срабатывания концевика
  if (REQUIRE_LIFT_BOTTOM_FOR_SAFE_CLOSE && !isLiftBottom()) {
    Serial.println("ОШИБКА: лифт не дошёл до нижнего концевика, крышку закрывать нельзя (безопасный режим).");
    return false;
  }

  // 2) Закрываем крышку
  if (!isLidClosed()) {
    if (!moveLidCloseToLimit()) {
      Serial.println("Прерывание при закрытии крышки.");
      return false;
    }
  } else {
    Serial.println("Крышка уже закрыта, пропускаем закрытие.");
  }

  Serial.println("СЕКВЕНЦИЯ: лифт внизу, крышка закрыта (базовое состояние).");
  return true;
}


// =========================================================
//             ВЕРХНЕУРОВНЕВЫЙ СЦЕНАРИЙ (ФАЗЫ)
// =========================================================

enum MainPhase {
  MAIN_WAIT_OPEN,      // база: крышка закрыта, лифт внизу
  MAIN_CENTERING_STAGE // крышка открыта, лифт поднят, идёт/ожидает центровка
};

MainPhase mainPhase = MAIN_WAIT_OPEN;

static bool      lastBootRawMain    = HIGH;
static unsigned long lastMainTrigMs = 0;
static unsigned long lastStatusPrint = 0;

// Детектор нажатия BOOT для верхнего уровня
bool bootPressedMain() {
  bool raw = digitalRead(PIN_BOOT);
  bool pressed = false;

  if (raw == LOW && lastBootRawMain == HIGH &&
      (millis() - lastMainTrigMs) > TRIGGER_DEBOUNCE_MS) {
    pressed = true;
    lastMainTrigMs = millis();
  }

  lastBootRawMain = raw;
  return pressed;
}

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

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

  Serial.println("\n\n=====================================");
  Serial.println("    ДРОН-ПОРТ: КРЫШКА + ЛИФТ + ЦЕНТРОВКА");
  Serial.println("=====================================\n");

  pinMode(PIN_BOOT,       INPUT_PULLUP);

  pinMode(LID_STEP_PIN,   OUTPUT);
  pinMode(LID_DIR_PIN,    OUTPUT);
 pinMode(LID_CLOSED_PIN, INPUT_PULLDOWN); 

  pinMode(LIFT_STEP_PIN,  OUTPUT);
  pinMode(LIFT_DIR_PIN,   OUTPUT);
pinMode(LIFT_BOTTOM_PIN, INPUT_PULLDOWN);

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

  // Центровка
  centeringInit();

  lastBootRawMain = digitalRead(PIN_BOOT);

  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()  ? "ДА" : "НЕТ");

  mainPhase = MAIN_WAIT_OPEN;

  Serial.println("\nБАЗА: лифт внизу, крышка закрыта.");
  Serial.println("Нажмите BOOT: крышка откроется, лифт поднимется и СРАЗУ начнётся центровка.");
}

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

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

  switch (mainPhase) {
    case MAIN_WAIT_OPEN: {
      if (bootPressedMain()) {
        Serial.println("\n[АКТИВАЦИЯ] BOOT: запуск последовательности открытия (крышка + лифт).");
        bool ok = sequenceOpenLidThenLiftUp();
        if (ok) {
          Serial.println("\n==> Крышка открыта, лифт поднят. АВТО-СТАРТ ЦЕНТРОВКИ.");
          // Автоматически запускаем центровку (первое движение)
          startCentering();
          mainPhase = MAIN_CENTERING_STAGE;
        } else {
          Serial.println("Последовательность открытия прервана. Остались в MAIN_WAIT_OPEN.");
        }
      }
      break;
    }

    case MAIN_CENTERING_STAGE: {
      // Здесь BOOT управляет ТОЛЬКО центровкой (через bootPressedCenter внутри).
      CenterState prevState = state;
      centeringLoopStep();

      // 1) Центровка (CENTERING -> WAIT_DECENTER): просто ждём, стол в центре.
      //    Ничего не делаем, ждём ещё одного BOOT для расцентровки.

      // 2) Расцентровка: DECENTERING -> WAIT_CENTER
      //    Как только расцентровка закончилась — опускаем лифт и закрываем крышку.
      if (prevState == DECENTERING && state == WAIT_CENTER) {
        Serial.println("\n==> ЦЕНТРОВКА: расцентровка завершена, запускаю закрытие порта (лифт вниз + крышка закрыть).");
        bool ok = sequenceLiftDownThenCloseLid();
        if (ok) {
          Serial.println("\n==> Порт вернулся в базовое состояние. Ждём BOOT для нового цикла.");
        } else {
          Serial.println("\n==> Закрытие прервано. Состояние смотреть по концевикам.");
        }
        mainPhase = MAIN_WAIT_OPEN;
      }
      break;
    }
  }

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

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

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

}