#include <Arduino.h>
#include "motor.h"
#include "utils.h"
#include "storage.h"
#include "ble.h"
#include "led.h"
#include "ir.h"
#include "tracking.h"
#include "mode.h"
#include "consts.h"

void setup()
{
  // 初始化口
  Serial.begin(115200);

  // 初始化 EEPROM
  Storage::init();

  // 初始化 BLE
  BLEManager::init(Storage::getName());

  // 初始化电机
  MotorController::init(
      {MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2},
      {MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2},
      {MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2},
      {MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2});

  // 初始化红外循迹模块
  IR::init();

  // 初始化超声波
  Ultrasonic::init({US_TRIG, US_ECHO}, US_SERVO_PIN);

  // 初始化状态灯
  if (STATUS_LED_ENABLE)
  {
    LED::init(STATUS_LED);
  }

  // 初始化循迹控制器
  TrackingController::init();
}

void loop()
{
  // 循环 1 部分，获取传感器数据
  IR::update();

  // 循环 2 部分，更新状态灯
  if (STATUS_LED_ENABLE)
  {
    LED::updateStatusLED(BLEManager::deviceConnected);
  }

  // 循环 3 部分，处理设备模式
  Mode::processDeviceMode();

  // 循环 4 部分，反馈状态
  if (BLEManager::deviceConnected)
  {
    sendStatus();
  }
}