// ======= ESP32-S3: УПРОЩЕННЫЙ ТЕСТ СИСТЕМЫ =======
// Тестирование всех компонентов по очереди
// Нажмите BOOT для запуска тестовой последовательности

#include <Arduino.h>
#include <ESP32Servo.h>

// ====================== НАСТРОЙКИ РЕВЕРСА ======================
// Центровка
static const bool M1_DIR_NORMAL = true;
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 bool LIFT_DIR_NORMAL = true;
static const bool MAG_DIR_NORMAL = true;
static const bool GRIP_DIR_NORMAL = true;

// Серво позиции
static const int SERVO_OPEN_US = 1050;
static const int SERVO_CLAMP_US = 1400;

// ====================== ПИНЫ ======================
#define PIN_BOOT 0

// Центровка - нижняя ось
#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 LIFT_STEP_PIN 13
#define LIFT_DIR_PIN 14
#define SERVO_PIN 15
#define GRIP_IN1_PIN 16
#define GRIP_IN2_PIN 17
#define MAG_STEP_PIN 18
#define MAG_DIR_PIN 35

// Концевики
#define LIM_N1_PIN 1

// ====================== ПАРАМЕТРЫ ======================
static const int STEPS_PER_MM = 100;
static const int BOTTOM_AXIS_MM = 254;
static const int TOP_AXIS_MM = 230;
static const int BOTTOM_AXIS_STEPS = BOTTOM_AXIS_MM * STEPS_PER_MM;
static const int TOP_AXIS_STEPS = TOP_AXIS_MM * STEPS_PER_MM;
static const int MAG_STEPS_90 = 50;

static const int STEP_PULSE_US = 3;
static const int CENTER_SPEED_SPS = 1200;
static const int LIFT_SPEED_SPS = 1500;
static const int MAG_SPEED_SPS = 800;

#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);
    digitalWrite(_dir, LOW);
    _intervalUs = SPS_TO_INTERVAL_US(sps);
    _pos = 0;
    _target = 0;
    _moving = false;
    _lastStepUs = micros();
  }

  long position() { return _pos; }
  bool isBusy() { return _moving; }
  void setPosition(long pos) { _pos = pos; }

  void moveTo(long pos) {
    _target = pos;
    _moving = (_target != _pos);
    updateDir();
  }

  void moveBy(long delta) {
    moveTo(_pos + delta);
  }

  void update() {
    if (!_moving) return;
    uint32_t now = micros();
    if ((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, _dir;
  bool _dirNormal;
  long _pos, _target;
  bool _moving;
  uint32_t _lastStepUs, _intervalUs;
};

// ====================== ГЛОБАЛЬНЫЕ ОБЪЕКТЫ ======================
SimpleStepper stepM1, stepM2, stepM3, stepM4, stepM5, stepM6;
SimpleStepper stepLift, stepMag;
Servo myServo;

enum TestState {
  IDLE,
  TEST_CENTER_FORWARD,
  TEST_CENTER_PAUSE,
  TEST_CENTER_BACK,
  TEST_MAG_ROTATE,
  TEST_LIFT_UP,
  TEST_SERVO_OPEN,
  TEST_SERVO_PAUSE,
  TEST_SERVO_CLOSE,
  TEST_GRIP_FORWARD,
  TEST_GRIP_REVERSE,
  TEST_COMPLETE
};

TestState testState = IDLE;
uint32_t stateStartMs = 0;
uint32_t pauseUntilMs = 0;

// ====================== ФУНКЦИИ ======================
void printStatus(const char* message) {
  Serial.println("=====================================");
  Serial.printf("ШАГ: %s\n", message);
  Serial.printf("Время: %lu мс\n", millis());
  Serial.println("=====================================");
}

void gotoState(TestState newState) {
  testState = newState;
  stateStartMs = millis();
}

void stopAllMotors() {
  // Остановка всех моторов центровки
  stepM1.moveTo(stepM1.position());
  stepM2.moveTo(stepM2.position());
  stepM3.moveTo(stepM3.position());
  stepM4.moveTo(stepM4.position());
  stepM5.moveTo(stepM5.position());
  stepM6.moveTo(stepM6.position());
  
  // Остановка моторов замены
  stepLift.moveTo(stepLift.position());
  stepMag.moveTo(stepMag.position());
  
  // Остановка редуктора
  gripSet(0);
  
  // Отключение серво
  if (myServo.attached()) {
    myServo.writeMicroseconds(SERVO_OPEN_US);
    delay(500);
    myServo.detach();
  }
  
  Serial.println("\n!!! ВСЕ МОТОРЫ ОСТАНОВЛЕНЫ !!!");
}

void resetSystem() {
  stopAllMotors();
  
  // Сброс позиций
  stepM1.setPosition(0);
  stepM2.setPosition(0);
  stepM3.setPosition(0);
  stepM4.setPosition(0);
  stepM5.setPosition(0);
  stepM6.setPosition(0);
  stepMag.setPosition(0);
  
  printStatus("СИСТЕМА СБРОШЕНА");
  Serial.println("Позиции всех моторов обнулены");
  Serial.println("Нажмите BOOT для запуска теста");
  
  testState = IDLE;
}

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

void gripSet(int direction) {
  // direction: 0=стоп, 1=вперед, 2=назад
  if (GRIP_DIR_NORMAL) {
    if (direction == 0) {
      digitalWrite(GRIP_IN1_PIN, LOW);
      digitalWrite(GRIP_IN2_PIN, LOW);
    } else if (direction == 1) {
      digitalWrite(GRIP_IN1_PIN, HIGH);
      digitalWrite(GRIP_IN2_PIN, LOW);
    } else {
      digitalWrite(GRIP_IN1_PIN, LOW);
      digitalWrite(GRIP_IN2_PIN, HIGH);
    }
  } else {
    if (direction == 0) {
      digitalWrite(GRIP_IN1_PIN, LOW);
      digitalWrite(GRIP_IN2_PIN, LOW);
    } else if (direction == 1) {
      digitalWrite(GRIP_IN1_PIN, LOW);
      digitalWrite(GRIP_IN2_PIN, HIGH);
    } else {
      digitalWrite(GRIP_IN1_PIN, HIGH);
      digitalWrite(GRIP_IN2_PIN, LOW);
    }
  }
}

// ====================== SETUP ======================
void setup() {
  Serial.begin(115200);
  delay(500);
  
  Serial.println("\n\n=====================================");
  Serial.println("ESP32-S3 ТЕСТОВАЯ ПРОШИВКА");
  Serial.println("Нажмите BOOT для запуска теста");
  Serial.println("=====================================\n");

  // Настройка пинов
  pinMode(PIN_BOOT, INPUT_PULLUP);
  pinMode(LIM_N1_PIN, INPUT_PULLUP);
  pinMode(GRIP_IN1_PIN, OUTPUT);
  pinMode(GRIP_IN2_PIN, OUTPUT);
  
  // Инициализация моторов
  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);
  stepLift.begin(LIFT_STEP_PIN, LIFT_DIR_PIN, LIFT_SPEED_SPS, LIFT_DIR_NORMAL);
  stepMag.begin(MAG_STEP_PIN, MAG_DIR_PIN, MAG_SPEED_SPS, MAG_DIR_NORMAL);
  
  // Серво
  myServo.attach(SERVO_PIN, 800, 2200);
  myServo.writeMicroseconds(SERVO_OPEN_US);
  delay(500);
  myServo.detach();
  
  // Редуктор стоп
  gripSet(0);
  
  Serial.println("Система готова к тестированию");
}

// ====================== LOOP ======================
void loop() {
  // Обновление всех моторов
  stepM1.update();
  stepM2.update();
  stepM3.update();
  stepM4.update();
  stepM5.update();
  stepM6.update();
  stepLift.update();
  stepMag.update();

  // Проверка второго нажатия BOOT во время теста для остановки
  if (testState != IDLE && testState != TEST_COMPLETE) {
    static bool lastBootState = HIGH;
    bool currentBootState = digitalRead(PIN_BOOT);
    
    if (currentBootState == LOW && lastBootState == HIGH) {
      delay(50); // дебаунс
      if (digitalRead(PIN_BOOT) == LOW) {
        Serial.println("\n=====================================");
        Serial.println("!!! ОСТАНОВКА ПО НАЖАТИЮ BOOT !!!");
        Serial.println("=====================================");
        resetSystem();
        lastBootState = LOW;
        return;
      }
    }
    lastBootState = currentBootState;
  }

  // Проверка кнопки BOOT в режиме ожидания
  if (testState == IDLE) {
    if (digitalRead(PIN_BOOT) == LOW) {
      delay(50); // дебаунс
      if (digitalRead(PIN_BOOT) == LOW) {
        printStatus("СТАРТ ТЕСТОВОЙ ПОСЛЕДОВАТЕЛЬНОСТИ");
        gotoState(TEST_CENTER_FORWARD);
      }
    }
    return;
  }

  // Выполнение тестовых шагов
  switch (testState) {
    
    case TEST_CENTER_FORWARD: {
      static bool moveStarted = false;
      
      if (!moveStarted && allMotorsIdle()) {
        printStatus("1. ЦЕНТРОВКА - движение к центру");
        Serial.println("М1,М4: движение CCW на 254мм");
        Serial.println("М2,М3: движение CW на 254мм");
        Serial.println("М5: движение CCW на 230мм");
        Serial.println("М6: движение CW на 230мм");
        
        stepM1.moveBy(-BOTTOM_AXIS_STEPS);
        stepM4.moveBy(-BOTTOM_AXIS_STEPS);
        stepM2.moveBy(+BOTTOM_AXIS_STEPS);
        stepM3.moveBy(+BOTTOM_AXIS_STEPS);
        stepM5.moveBy(-TOP_AXIS_STEPS);
        stepM6.moveBy(+TOP_AXIS_STEPS);
        moveStarted = true;
      }
      
      if (moveStarted && allMotorsIdle()) {
        Serial.println("Центровка завершена, пауза 1 сек");
        pauseUntilMs = millis() + 1000;
        moveStarted = false;
        gotoState(TEST_CENTER_PAUSE);
      }
      break;
    }
    
    case TEST_CENTER_PAUSE:
      if (millis() >= pauseUntilMs) {
        gotoState(TEST_CENTER_BACK);
      }
      break;
    
    case TEST_CENTER_BACK: {
      static bool moveBackStarted = false;
      
      if (!moveBackStarted && allMotorsIdle()) {
        printStatus("2. ЦЕНТРОВКА - возврат на исходную");
        Serial.println("Все моторы возвращаются в позицию 0");
        
        stepM1.moveTo(0);
        stepM2.moveTo(0);
        stepM3.moveTo(0);
        stepM4.moveTo(0);
        stepM5.moveTo(0);
        stepM6.moveTo(0);
        moveBackStarted = true;
      }
      
      if (moveBackStarted && allMotorsIdle()) {
        // Проверяем что все моторы действительно в нуле
        if (stepM1.position() == 0 && stepM2.position() == 0 && 
            stepM3.position() == 0 && stepM4.position() == 0 &&
            stepM5.position() == 0 && stepM6.position() == 0) {
          Serial.println("Центровка завершена");
          moveBackStarted = false;
          gotoState(TEST_MAG_ROTATE);
        }
      }
      break;
    }
    
    case TEST_MAG_ROTATE: {
      static bool magStarted = false;
      static uint32_t magStartTime = 0;
      
      if (!magStarted) {
        printStatus("3. МАГАЗИН - поворот на 90 градусов");
        Serial.printf("Поворот на %d шагов\n", MAG_STEPS_90);
        Serial.printf("Текущая позиция магазина: %ld\n", stepMag.position());
        stepMag.moveBy(MAG_STEPS_90);
        magStarted = true;
        magStartTime = millis();
      }
      
      // Даем время на движение и проверяем завершение
      if (magStarted && (millis() - magStartTime > 500) && !stepMag.isBusy()) {
        Serial.printf("Поворот магазина завершен. Новая позиция: %ld\n", stepMag.position());
        magStarted = false;
        gotoState(TEST_LIFT_UP);
      }
      
      // Таймаут для магазина
      if (magStarted && (millis() - magStartTime > 5000)) {
        Serial.println("Таймаут поворота магазина, переход к лифту");
        magStarted = false;
        gotoState(TEST_LIFT_UP);
      }
      break;
    }
    
    case TEST_LIFT_UP: {
      static bool liftStarted = false;
      static bool limitReached = false;
      
      // Проверяем концевик
      if (digitalRead(LIM_N1_PIN) == LOW && !limitReached) {
        // Немедленно останавливаем мотор
        stepLift.moveTo(stepLift.position());
        limitReached = true;
        printStatus("4. ЛИФТ достиг концевика N1");
        Serial.printf("Позиция лифта при N1: %ld\n", stepLift.position());
        stepLift.setPosition(0);
        Serial.println("Позиция лифта обнулена");
      }
      
      // Запускаем движение только если концевик не достигнут
      if (!limitReached && !liftStarted) {
        printStatus("4. ЛИФТ - подъем до концевика N1");
        Serial.println("Движение вверх до срабатывания N1");
        stepLift.moveBy(-100000); // большое число для движения вверх
        liftStarted = true;
      }
      
      // Переход к следующему состоянию после достижения концевика и остановки
      if (limitReached && !stepLift.isBusy()) {
        liftStarted = false;
        limitReached = false;
        gotoState(TEST_SERVO_OPEN);
      }
      
      // Таймаут
      if (!limitReached && millis() - stateStartMs > 20000) {
        Serial.println("ОШИБКА: Таймаут подъема лифта!");
        stepLift.moveTo(stepLift.position());
        liftStarted = false;
        limitReached = false;
        gotoState(TEST_SERVO_OPEN);
      }
      break;
    }
    
    case TEST_SERVO_OPEN:
      printStatus("5. СЕРВО - открытие");
      Serial.printf("Установка позиции %d мкс\n", SERVO_OPEN_US);
      myServo.attach(SERVO_PIN, 800, 2200);
      myServo.writeMicroseconds(SERVO_OPEN_US);
      pauseUntilMs = millis() + 1000;
      gotoState(TEST_SERVO_PAUSE);
      break;
    
    case TEST_SERVO_PAUSE:
      if (millis() >= pauseUntilMs) {
        gotoState(TEST_SERVO_CLOSE);
      }
      break;
    
    case TEST_SERVO_CLOSE:
      printStatus("6. СЕРВО - закрытие");
      Serial.printf("Установка позиции %d мкс\n", SERVO_CLAMP_US);
      myServo.writeMicroseconds(SERVO_CLAMP_US);
      pauseUntilMs = millis() + 1000;
      gotoState(TEST_GRIP_FORWARD);
      break;
    
    case TEST_GRIP_FORWARD:
      if (millis() >= pauseUntilMs) {
        myServo.detach();
        printStatus("7. РЕДУКТОР - вращение вперед 1 сек");
        gripSet(1);
        pauseUntilMs = millis() + 1000;
        gotoState(TEST_GRIP_REVERSE);
      }
      break;
    
    case TEST_GRIP_REVERSE:
      if (millis() >= pauseUntilMs) {
        printStatus("8. РЕДУКТОР - вращение назад 1 сек");
        gripSet(2);
        pauseUntilMs = millis() + 1000;
        gotoState(TEST_COMPLETE);
      }
      break;
    
    case TEST_COMPLETE:
      if (millis() >= pauseUntilMs) {
        gripSet(0);
        printStatus("ТЕСТ ЗАВЕРШЕН!");
        Serial.println("\nВсе компоненты протестированы:");
        Serial.println("✓ Центровка (6 моторов)");
        Serial.println("✓ Магазин");
        Serial.println("✓ Лифт с концевиком N1");
        Serial.println("✓ Сервопривод");
        Serial.println("✓ Редуктор");
        Serial.println("\nНажмите BOOT для повтора теста");
        gotoState(IDLE);
      }
      break;
  }
}