#include <Arduino.h>

#include "core/display/body/body.h"
#include "core/servo/servo.h"
#include "core/ultrasonic/ultrasonic.h"
#include "core/display/face/face.h"
#include "core/robot_state/robot_state.h"
#include "core/joystick/joystick.h"

#include "core/actuators/actuators.h"
#include "server/web_server.h"
#include "server/ws_server.h"
#include "core/controller/controller.h"
// #include "core/deprecate/ir_input/ir_input.h"

#define TOTAL_STAGES 5

void runStartupChecks() {
  // Этап 1: Проверка джойстика
  oledSetStage(1, TOTAL_STAGES);
  bool joyOk = oledCheckJoystick();
  oledShowMain("Checking devices", "Stage 1/5", 
               joyOk ? "Joystick: OK" : "Joystick: NOT FOUND",
               "");
  delay(500);
  
  // TODO: Этап 2-5 (другие проверки)
  oledSetStage(2, TOTAL_STAGES);
  delay(500);
  
  oledSetStage(3, TOTAL_STAGES);
  delay(500);
  
  oledSetStage(4, TOTAL_STAGES);
  delay(500);
  
  oledSetStage(5, TOTAL_STAGES);
  delay(500);
  
  // Все проверки пройдены
  oledSetSystemReady(joyOk);  // пока только джойстик влияет
}

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

  oledInit();
  servoInit();
  ultrasonicInit();
  joyInit();
  // faceInit();
  actuatorsInit();
  webServerInit();
  wsInit();
  // irInit();
  
  // Запуск проверок
  runStartupChecks();

  robot.lastCmdMs = millis();
}

void loop() {
  webServerLoop();
  wsLoop();
  joyUpdate();
  controllerUpdate();
  // irPoll();
}