#include <Arduino.h>
#include <BluetoothSerial.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 // 速度增量

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

BluetoothSerial SerialBT;

// 在全局变量定义区域添加以下内容
#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

// 数据包缓冲区
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 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;
}

u_char getSerialBTStatus()
{
  return SerialBT.connected() ? 0x01 : 0x00;
}

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

  // 初始化蓝牙
  SerialBT.begin("WhiteTiger");

  // 设置引脚模式
  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, Stream &serial)
{
  if (length < 4)
    return; // 最小包长度：包头(1) + 长度(1) + 指令(1) + 包尾(1)

  uint8_t packetLength = packet[1]; // 获取长度
  uint8_t cmd = packet[2];          // 获取指令
  uint8_t direction, speed, time;
  float distance;

  uint8_t transmitBuffer[PACKET_MAX_LENGTH];

  switch (cmd)
  {
  case CMD_GET_BT_STATUS:
    Serial.println("CMD_GET_BT_STATUS");
    // 回复状态
    serial.write(PACKET_T_HEAD);
    serial.write(0x05); // 包体长度
    serial.write(CMD_GET_BT_STATUS);
    serial.write(getSerialBTStatus());
    serial.write(PACKET_T_TAIL);
    break;

  case CMD_GET_DISTANCE:
    Serial.println("CMD_GET_DISTANCE");
    distance = getDistance();
    Serial.printf("distance = %f\n", distance);
    floatToBytes(distance, transmitBuffer);
    // 回复距离
    serial.write(PACKET_T_HEAD);
    serial.write(0x08); // 包体长度
    serial.write(CMD_GET_DISTANCE);
    serial.write(transmitBuffer, 4);
    serial.write(PACKET_T_TAIL);
    break;

  case CMD_MOTOR_MOVE_CONTROL:
    // 获取方向和速度
    direction = packet[3];
    speed = packet[4];

    Serial.printf("CMD_MOTOR_MOVE_CONTROL: direction = 0x%02X, speed = %d\n", direction, 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);
    }

    // 前进
    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);
    }

    // 后退
    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.printf("CMD_MOTOR_ROTATE_CONTROL: direction = 0x%02X, time = %d\n", direction, 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); // 右前
    }

    // 逆时针
    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, Stream &serial)
{
  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, serial);
        }
      }
    }
    else
    {
      isReceivingPacket = false;
    }
  }
}

void updateStatusLED()
{
  if (SerialBT.connected())
  {
    // 蓝牙已连接，LED常亮
    digitalWrite(STATUS_LED, HIGH);
  }
  else
  {
    // 蓝牙未连接，LED慢闪
    unsigned long currentMillis = millis();
    if (currentMillis - lastLedToggle >= LED_FLASH_INTERVAL)
    {
      lastLedToggle = currentMillis;
      ledState = !ledState;
      digitalWrite(STATUS_LED, ledState);
    }
  }
}

void loop()
{
  if (SerialBT.available())
  {
    uint8_t incomingByte = SerialBT.read();
    processSerialIncomingByte(incomingByte, SerialBT);
    Serial.printf("BT Received: 0x%02X\n", incomingByte);
  }

  if (Serial.available())
  {
    uint8_t incomingByte = Serial.read();
    processSerialIncomingByte(incomingByte, Serial);
    Serial.printf("UART Received: 0x%02X\n", incomingByte);
  }

  updateStatusLED(); // 更新LED状态
}