#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

#define STATUS_LED 2

#define HC_SR04_TRIG 23
#define HC_SR04_ECHO 22

// 使用 2 片 TB6612 控制 4 个电机
// 使用差速控制转向

// TB6612 #1 控制 A B 电机
// A 电机 左前轮
#define MOTOR_A_PWMA 21 // 电机 A PWM
#define MOTOR_A_AIN1 18 // 电机 A 方向控制1
#define MOTOR_A_AIN2 19 // 电机 A 方向控制2

// B 电机 左后轮
#define MOTOR_B_PWMB 5  // 电机 B PWM
#define MOTOR_B_BIN1 16 // 电机 B 方向控制1
#define MOTOR_B_BIN2 17 // 电机 B 方向控制2

// TB6612 #2 控制 C D 电机
// C 电机 右后轮
#define MOTOR_C_PWMA 4  // 电机 C PWM
#define MOTOR_C_AIN1 15 // 电机 C 方向控制1
#define MOTOR_C_AIN2 2  // 电机 C 方向控制2

// D 电机 右前轮
#define MOTOR_D_PWMB 32 // 电机 D PWM
#define MOTOR_D_BIN1 25 // 电机 D 方向控制1
#define MOTOR_D_BIN2 33 // 电机 D 方向控制2

// 运动控制参数
#define MAX_SPEED 255      // 最大速度
#define BASE_SPEED 200     // 基础速度
#define MIN_SPEED 50       // 最小速度
#define TURN_RATIO 0.6     // 转向时的速度比例
#define SPEED_INCREMENT 10 // 速度增量

// Nordic UART Service UUID
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

#define PACKET_R_HEAD 0x00
#define PACKET_R_TAIL 0xFF
#define PACKET_T_HEAD 0x01
#define PACKET_T_TAIL 0xFE
#define PACKET_MAX_LENGTH 32 // 数据包最大长度

// 指令定义
#define CMD_GET_BT_STATUS 0x10
#define CMD_GET_DISTANCE 0x11
#define CMD_MOTOR_MOVE_CONTROL 0x20
#define CMD_MOTOR_ROTATE_CONTROL 0x21

// 全局变量
int currentSpeed = 0;   // 当前速度
int turnOffset = 0;     // 转向偏移量 (-100 到 100)
bool isMoving = false;  // 运动状态
bool isTurning = false; // 转向状态

// BLE 相关
BLEServer *pServer = nullptr;
BLECharacteristic *pTxCharacteristic = nullptr;
bool deviceConnected = false;

// 数据包缓冲区
uint8_t packetBuffer[PACKET_MAX_LENGTH];
int packetIndex = 0;
bool isReceivingPacket = false;

// 在全局变量定义区域添加
#define LED_FLASH_INTERVAL 1000  // LED闪烁间隔(ms)
unsigned long lastLedToggle = 0; // 上次LED切换状态的时间
bool ledState = false;           // LED当前状态

void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic);

class CarBLEServerCallbacks : public BLEServerCallbacks
{
  void onConnect(BLEServer *pServer)
  {
    deviceConnected = true;
  };

  void onDisconnect(BLEServer *pServer)
  {
    deviceConnected = false;
    // 重新开始广播
    pServer->getAdvertising()->start();
  }
};

class CarBLECharacteristicCallbacks : public BLECharacteristicCallbacks
{
  void onWrite(BLECharacteristic *pCharacteristic)
  {
    std::string rxValue = pCharacteristic->getValue();
    if (rxValue.length() > 0)
    {
      for (int i = 0; i < rxValue.length(); i++)
      {
        processSerialIncomingByte((uint8_t)rxValue[i], *pTxCharacteristic);
      }
    }
  }
};

void floatToBytes(float val, uint8_t *bytes)
{
  union
  {
    float f;
    uint8_t bytes[4];
  } u;
  u.f = val;

  // 考虑大小端问题
  for (int i = 0; i < 4; i++)
  {
    bytes[i] = u.bytes[i];
  }
}

// 电机控制函数
void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed)
{
  if (speed > 0)
  {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
  }
  else if (speed < 0)
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
    speed = -speed;
  }
  else
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
  }
  analogWrite(pwmPin, speed);
}

float getDistance()
{
  digitalWrite(HC_SR04_TRIG, HIGH);
  delayMicroseconds(1);
  digitalWrite(HC_SR04_TRIG, LOW);
  float distance = pulseIn(HC_SR04_ECHO, HIGH); // 计数接收高电平时间
  distance = distance * 340 / 2 / 10000;        // 计算距离 1：声速：340M/S  2：实际距离为1/2声速距离 3：计数时钟为1US//温补公式：c=(331.45+0.61t/℃)m•s-1 (其中331.45是在0度）
  return distance;
}

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

  // 初始化 BLE
  BLEDevice::init("WhiteTiger");
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new CarBLEServerCallbacks());

  BLEService *pService = pServer->createService(SERVICE_UUID);

  // 创建 RX 特征值 (用于接收数据)
  BLECharacteristic *pRxCharacteristic = pService->createCharacteristic(
      CHARACTERISTIC_UUID_RX,
      BLECharacteristic::PROPERTY_WRITE);
  pRxCharacteristic->setCallbacks(new CarBLECharacteristicCallbacks());

  // 创建 TX 特征值 (用于发送数据)
  pTxCharacteristic = pService->createCharacteristic(
      CHARACTERISTIC_UUID_TX,
      BLECharacteristic::PROPERTY_NOTIFY);
  pTxCharacteristic->addDescriptor(new BLE2902());

  pService->start();
  pServer->getAdvertising()->start();

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

  pinMode(MOTOR_A_PWMA, OUTPUT);
  pinMode(MOTOR_A_AIN1, OUTPUT);
  pinMode(MOTOR_A_AIN2, OUTPUT);

  pinMode(MOTOR_B_PWMB, OUTPUT);
  pinMode(MOTOR_B_BIN1, OUTPUT);
  pinMode(MOTOR_B_BIN2, OUTPUT);

  pinMode(MOTOR_C_PWMA, OUTPUT);
  pinMode(MOTOR_C_AIN1, OUTPUT);
  pinMode(MOTOR_C_AIN2, OUTPUT);

  pinMode(MOTOR_D_PWMB, OUTPUT);
  pinMode(MOTOR_D_BIN1, OUTPUT);
  pinMode(MOTOR_D_BIN2, OUTPUT);

  pinMode(HC_SR04_TRIG, OUTPUT);
  pinMode(HC_SR04_ECHO, INPUT);
}

void handleSerialPacket(uint8_t *packet, int length, BLECharacteristic &characteristic)
{
  if (!deviceConnected || length < 4)
    return;

  uint8_t packetLength = packet[1]; // 获取长度
  uint8_t cmd = packet[2];          // 获取指令
  uint8_t direction, speed, time;
  float distance;
  uint8_t buffer[PACKET_MAX_LENGTH]; // 用于存储要发送的数据
  int bufferIndex = 0;

  switch (cmd)
  {
  case CMD_GET_BT_STATUS:
    Serial.println("CMD_GET_BT_STATUS");
    // 构建响应数据
    buffer[0] = PACKET_T_HEAD;
    buffer[1] = 0x05;
    buffer[2] = CMD_GET_BT_STATUS;
    buffer[3] = (uint8_t)(deviceConnected ? 0x01 : 0x00);
    buffer[4] = PACKET_T_TAIL;
    characteristic.setValue(buffer, 5);
    characteristic.notify();
    break;

  case CMD_GET_DISTANCE:
    distance = getDistance();
    Serial.println("CMD_GET_DISTANCE, distance: " + String(distance));
    // 构建响应数据
    buffer[0] = PACKET_T_HEAD;
    buffer[1] = 0x08;
    buffer[2] = CMD_GET_DISTANCE;
    floatToBytes(distance, &buffer[3]);
    buffer[7] = PACKET_T_TAIL;
    characteristic.setValue(buffer, 8);
    characteristic.notify();
    break;

  case CMD_MOTOR_MOVE_CONTROL:
    direction = packet[3];
    speed = packet[4];
    Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed));
    // 移动
    if (direction == 0x00)
    {
      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 0);
      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2, 0);
      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2, 0);
      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 0);
    }
    // 前进
    else if (direction == 0x01)
    {
      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, speed);
      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, speed);
      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, speed);
      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, speed);
    }
    // 后退
    else if (direction == 0x02)
    {
      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -speed);
      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -speed);
      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -speed);
      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -speed);
    }
    break;

  case CMD_MOTOR_ROTATE_CONTROL:
    direction = packet[3];
    time = packet[4];
    Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time));
    // 顺时针
    if (direction == 0x00)
    {
      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, 255);
      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, 255);
      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, -255);
      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, -255);
    }
    // 逆时针
    else if (direction == 0x01)
    {
      motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, -255);
      motorControl(MOTOR_B_PWMB, MOTOR_B_BIN2, MOTOR_B_BIN1, -255);
      motorControl(MOTOR_C_PWMA, MOTOR_C_AIN2, MOTOR_C_AIN1, 255);
      motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, 255);
    }
    break;

  default:
    break;
  }
}

void processSerialIncomingByte(uint8_t incomingByte, BLECharacteristic &characteristic)
{
  static uint8_t expectedLength = 0;

  packetIndex++;

  if (incomingByte == PACKET_R_HEAD && !isReceivingPacket)
  {
    isReceivingPacket = true;
    packetIndex = 0;
    packetBuffer[packetIndex] = incomingByte;
  }
  else if (isReceivingPacket)
  {
    if (packetIndex < PACKET_MAX_LENGTH)
    {
      packetBuffer[packetIndex] = incomingByte;

      // 第二个字节是包体长度
      if (packetIndex == 1)
      {
        expectedLength = incomingByte;
      }

      // 到达预期长度时检查包尾
      if (packetIndex == expectedLength - 1)
      {
        isReceivingPacket = false;
        if (incomingByte == PACKET_R_TAIL)
        {
          handleSerialPacket(packetBuffer, packetIndex + 1, *pTxCharacteristic);
        }
      }
    }
    else
    {
      isReceivingPacket = false;
    }
  }
}

void updateStatusLED()
{
  if (deviceConnected)
  {
    digitalWrite(STATUS_LED, HIGH);
  }
  else
  {
    unsigned long currentMillis = millis();
    if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL)
    {
      lastLedToggle = currentMillis;
      ledState = !ledState;
      digitalWrite(STATUS_LED, ledState);
    }
  }
}

void loop()
{
  updateStatusLED();
}