#include <Arduino.h>
#include <motor.h>
#include <utils.h>
#include <storage.h>
#include <ble.h>
#include "consts.h"

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

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

  // 初始化 BLE
  BLEManager::init(DEVICE_NAME);

  // 初始化电机
  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});

  // 设置引脚模式
  pinMode(STATUS_LED, OUTPUT);
  digitalWrite(STATUS_LED, HIGH);
}

void loop()
{
  updateStatusLED(BLEManager::deviceConnected, STATUS_LED);
  if (BLEManager::deviceConnected)
  {
    sendStatus(*BLEManager::pTxCharacteristic);
  }
}