#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <SPIFFS.h>
#include "consts.h"

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

#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_SPIFFS_STATUS 0x11
#define CMD_GET_DISTANCE 0x12
#define CMD_MOTOR_MOVE_CONTROL 0x20
#define CMD_MOTOR_STEER_CONTROL 0x21
#define CMD_MOTOR_SINGLE_CONTROL 0x22
#define CMD_MOTOR_ROTATE_CONTROL 0x23
#define CMD_DEMO_PID 0xf0
#define CMD_DEMO_PATH 0xf1
#define CMD_MOTOR_XYR_CONTROL 0x24  // XYR轴向控制指令

#define CMD_STATUS_MOTOR 0xE0

// 全局变量
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当前状态

#define MOTOR_STATUS_INTERVAL 200        // 发送电机状态的时间间隔(ms)
unsigned long lastMotorStatusUpdate = 0; // 上次发送电机状态的时间

// 在全局变量区域添加电机方向状态跟踪
struct MotorStatus
{
  bool in1;
  bool in2;
  unsigned char pwm;
};

// 定义四个电机的状态
MotorStatus motorA = {false, false, 0};
MotorStatus motorB = {false, false, 0};
MotorStatus motorC = {false, false, 0};
MotorStatus motorD = {false, false, 0};

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 pwmControl(char motor, unsigned char pwm)
{
  int pwmPin;
  switch (motor)
  {
  case 'A':
    pwmPin = MOTOR_A_PWMA;
    motorA.pwm = pwm;
    break;
  case 'B':
    pwmPin = MOTOR_B_PWMB;
    motorB.pwm = pwm;
    break;
  case 'C':
    pwmPin = MOTOR_C_PWMA;
    motorC.pwm = pwm;
    break;
  case 'D':
    pwmPin = MOTOR_D_PWMB;
    motorD.pwm = pwm;
    break;
  }
  analogWrite(pwmPin, pwm);
}

// 电机控制函数
void motorControl(char motor, int speed)
{
  int in1Pin, in2Pin;
  MotorStatus *status;

  switch (motor)
  {
  case 'A':
    in1Pin = MOTOR_A_AIN1;
    in2Pin = MOTOR_A_AIN2;
    status = &motorA;
    break;
  case 'B':
    in1Pin = MOTOR_B_BIN2;
    in2Pin = MOTOR_B_BIN1;
    status = &motorB;
    break;
  case 'C':
    in1Pin = MOTOR_C_AIN2;
    in2Pin = MOTOR_C_AIN1;
    status = &motorC;
    break;
  case 'D':
    in1Pin = MOTOR_D_BIN1;
    in2Pin = MOTOR_D_BIN2;
    status = &motorD;
    break;
  }

  if (speed > 0)
  {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
    if (motor == 'A' || motor == 'D')
    {
      status->in1 = true;
      status->in2 = false;
    }
    else
    {
      status->in1 = false;
      status->in2 = true;
    }
  }
  else if (speed < 0)
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
    if (motor == 'A' || motor == 'D')
    {
      status->in1 = false;
      status->in2 = true;
    }
    else
    {
      status->in1 = true;
      status->in2 = false;
    }

    speed = -speed;
  }
  else
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
    status->in1 = false;
    status->in2 = false;
  }
  pwmControl(motor, speed);
}

void motorXYRControl(int8_t x, int8_t y, int8_t r) {
  // 将输入范围限制在 -100 到 100
  x = constrain(x, -100, 100);
  y = constrain(y, -100, 100);
  r = constrain(r, -100, 100);
  
  // 将 -100~100 映射到 -255~255
  int16_t mappedX = map(x, -100, 100, -255, 255);
  int16_t mappedY = map(y, -100, 100, -255, 255);
  int16_t mappedR = map(r, -100, 100, -255, 255);
  
  // 麦克纳姆轮运动学方程
  // 左前轮 = Y + X + R
  // 右前轮 = Y - X - R
  // 右后轮 = Y + X - R
  // 左后轮 = Y - X + R
  int16_t speedA = mappedY + mappedX + mappedR;  // 左前轮
  int16_t speedB = mappedY - mappedX - mappedR;  // 右前轮
  int16_t speedC = mappedY + mappedX - mappedR;  // 右后轮
  int16_t speedD = mappedY - mappedX + mappedR;  // 左后轮
  
  // 找出最大速度的绝对值
  int16_t maxSpeed = max(max(abs(speedA), abs(speedB)), 
                        max(abs(speedC), abs(speedD)));
  
  // 如果最大速度超过255，等比例缩放所有速度
  if (maxSpeed > 255) {
    float scale = 255.0f / maxSpeed;
    speedA *= scale;
    speedB *= scale;
    speedC *= scale;
    speedD *= scale;
  }
  
  // 控制各个电机
  motorControl('A', speedA);
  motorControl('B', speedB);
  motorControl('C', speedC);
  motorControl('D', speedD);
}


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;
}

class CarStorage
{
public:
  static bool isMounted;

  static void init()
  {
    if (!SPIFFS.begin(true))
    {
      Serial.println("SPIFFS Mount Failed");
      isMounted = false;
    }
    else
      isMounted = true;
  }

  static unsigned int getSensitivity()
  {
    if (!isMounted)
      return 0xFF;

    File file = SPIFFS.open("/sensitivity.txt", "r");
    if (!file)
      return 0xFF;

    String sensitivity = file.readStringUntil('\n');
    file.close();

    return sensitivity.toInt();
  }

  static void setSensitivity(unsigned int sensitivity)
  {
    if (!isMounted)
      return;

    File file = SPIFFS.open("/sensitivity.txt", "w");
    file.println(sensitivity);
    file.close();
  }
};
bool CarStorage::isMounted = false;

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

  // 初始化 BLE
  BLEDevice::init(DEVICE_NAME);
  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();

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

  // 设置引脚模式
  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, wheel, diffSpeed;
  float distance;
  uint8_t buffer[PACKET_MAX_LENGTH];
  int bufferIndex = 0;

  // PID 相关变量声明移到这里
  float Kp, Ki, Kd;
  float targetSpeed, currentSpeed, lastError, integral;
  unsigned long lastTime;
  unsigned long startTime;

  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_SPIFFS_STATUS:
    Serial.println("CMD_GET_SPIFFS_STATUS");
    // 构建响应数据
    buffer[0] = PACKET_T_HEAD;
    buffer[1] = 0x05;
    buffer[2] = CMD_GET_SPIFFS_STATUS;
    buffer[3] = (uint8_t)(CarStorage::isMounted ? 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('A', 0);
      motorControl('B', 0);
      motorControl('C', 0);
      motorControl('D', 0);
    }
    // 前进
    else if (direction == 0x01)
    {
      motorControl('A', speed);
      motorControl('B', speed);
      motorControl('C', speed);
      motorControl('D', speed);
    }
    // 后退
    else if (direction == 0x02)
    {
      motorControl('A', -speed);
      motorControl('B', -speed);
      motorControl('C', -speed);
      motorControl('D', -speed);
    }
    break;

  case CMD_MOTOR_STEER_CONTROL:
    direction = packet[3];
    diffSpeed = packet[4];
    Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed));
    // 左转
    if (direction == 0x00)
    {
      motorControl('A', motorA.pwm - diffSpeed);
      motorControl('B', motorB.pwm - diffSpeed);
    }
    // 右转
    else if (direction == 0x01)
    {
      motorControl('C', motorC.pwm - diffSpeed);
      motorControl('D', motorD.pwm - diffSpeed);
    }
    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('A', 255);
      motorControl('B', 255);
      motorControl('C', -255);
      motorControl('D', -255);
    }
    // 逆时针
    else if (direction == 0x01)
    {
      motorControl('A', -255);
      motorControl('B', -255);
      motorControl('C', 255);
      motorControl('D', 255);
    }
    break;

  case CMD_MOTOR_SINGLE_CONTROL:
    wheel = packet[3];
    direction = packet[4];
    speed = packet[5];
    Serial.println("CMD_MOTOR_SINGLE_CONTROL, wheel: " + String(wheel) + ", direction: " + String(direction) + ", speed: " + String(speed));
    // 单轮控制
    switch (wheel)
    {
    case 0x00:
      motorControl('A', speed);
      break;
    case 0x01:
      motorControl('B', speed);
      break;
    case 0x02:
      motorControl('C', speed);
      break;
    case 0x03:
      motorControl('D', speed);
      break;
    }
    break;

  case CMD_DEMO_PID:
    Serial.println("CMD_DEMO_PID");

    // PID 参数
    Kp = 2.0;
    Ki = 0.5;
    Kd = 0.1;

    targetSpeed = 255;
    currentSpeed = 0;
    lastError = 0;
    integral = 0;
    lastTime = millis();

    // 加速阶段 - PID控制
    while (millis() - lastTime < 2000)
    { // 2秒加速过程
      float error = targetSpeed - currentSpeed;
      integral += error;
      float derivative = error - lastError;

      float output = Kp * error + Ki * integral + Kd * derivative;
      output = constrain(output, 0, 255); // 限制输出范围

      currentSpeed = output;

      // 应用到所有电机
      motorControl('A', output);
      motorControl('B', output);
      motorControl('C', output);
      motorControl('D', output);

      lastError = error;
      delay(10); // 控制周期
    }

    // 停止阶段 - PID控制
    lastTime = millis();
    targetSpeed = 0; // 目标速度设为0

    while (millis() - lastTime < 1000)
    { // 1秒减速过程
      float error = targetSpeed - currentSpeed;
      integral += error;
      float derivative = error - lastError;

      float output = Kp * error + Ki * integral + Kd * derivative;
      output = constrain(output, 0, 255);

      currentSpeed = output;

      // 应用到所有电机
      motorControl('A', output);
      motorControl('B', output);
      motorControl('C', output);
      motorControl('D', output);

      lastError = error;
      delay(10);
    }

    // 确保完全停止
    motorControl('A', 0);
    motorControl('B', 0);
    motorControl('C', 0);
    motorControl('D', 0);
    break;

  case CMD_DEMO_PATH:
    Serial.println("CMD_DEMO_PATH");

    // 第一段：全速前进1秒
    currentSpeed = 255;
    startTime = millis();
    // 第一段：全速前进1秒
    while (millis() - startTime < 1000)
    {
      motorControl('A', currentSpeed);
      motorControl('B', currentSpeed);
      motorControl('C', currentSpeed);
      motorControl('D', currentSpeed);
      delay(10);
    }

    // 第二段：左转90度
    startTime = millis();
    while (millis() - startTime < 500)
    { // 假设500ms可以转90度
      // 左侧电机反转,右侧电机正转
      motorControl('A', 200);
      motorControl('B', 200);
      motorControl('C', 200);
      motorControl('D', 200);
      delay(10);
    }

    // 第三段：全速前进1秒
    startTime = millis();
    while (millis() - startTime < 1000)
    {
      motorControl('A', currentSpeed);
      motorControl('B', currentSpeed);
      motorControl('C', currentSpeed);
      motorControl('D', currentSpeed);
      delay(10);
    }

    // 最后停止所有电机
    motorControl('A', 0);
    motorControl('B', 0);
    motorControl('C', 0);
    motorControl('D', 0);

    break;

  case CMD_MOTOR_XYR_CONTROL:
    if (length >= 7) {  // 确保数据包长度正确
      int8_t x = (int8_t)packet[3];  // X轴向速度
      int8_t y = (int8_t)packet[4];  // Y轴向速度
      int8_t r = (int8_t)packet[5];  // 旋转速度
      
      Serial.printf("CMD_MOTOR_XYR_CONTROL: X=%d, Y=%d, R=%d\n", x, y, r);
      motorXYRControl(x, y, r);
    }
    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 sendMotorStatus()
{
  unsigned long currentMillis = millis();
  if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
  {
    lastMotorStatusUpdate = currentMillis;

    uint8_t buffer[PACKET_MAX_LENGTH];
    buffer[0] = PACKET_T_HEAD;
    buffer[1] = 0x0C;
    buffer[2] = CMD_STATUS_MOTOR;
    buffer[3] = (motorA.in2 << 1) | motorA.in1;
    buffer[4] = motorA.pwm;
    buffer[5] = (motorB.in2 << 1) | motorB.in1;
    buffer[6] = motorB.pwm;
    buffer[7] = (motorC.in2 << 1) | motorC.in1;
    buffer[8] = motorC.pwm;
    buffer[9] = (motorD.in2 << 1) | motorD.in1;
    buffer[10] = motorD.pwm;
    buffer[11] = PACKET_T_TAIL;
    pTxCharacteristic->setValue(buffer, 12);
    pTxCharacteristic->notify();
  }
}

void loop()
{
  updateStatusLED();
  if (deviceConnected)
  {
    sendMotorStatus();
  }
}