#include "handlers.h"

uint8_t buffer[PACKET_MAX_LENGTH];

void Handlers::getBTStatus(BLECharacteristic &characteristic)
{
  Serial.println("CMD_GET_BT_STATUS");
  // 构建响应数据
  buffer[0] = PACKET_T_HEAD;
  buffer[1] = 0x05;
  buffer[2] = CMD_GET_BT_STATUS;
  buffer[3] = (uint8_t)(BLEManager::deviceConnected ? 0x01 : 0x00);
  buffer[4] = PACKET_T_TAIL;
  characteristic.setValue(buffer, 5);
  characteristic.notify();
}

void Handlers::getSPIFFSStatus(BLECharacteristic &characteristic)
{
  Serial.println("CMD_GET_SPIFFS_STATUS");
  // 构建响应数据
  buffer[0] = PACKET_T_HEAD;
  buffer[1] = 0x05;
  buffer[2] = CMD_GET_SPIFFS_STATUS;
  buffer[3] = (uint8_t)(Storage::isMounted ? 0x01 : 0x00);
  buffer[4] = PACKET_T_TAIL;
  characteristic.setValue(buffer, 5);
  characteristic.notify();
}

void Handlers::getDistance(BLECharacteristic &characteristic)
{
  float distance = Ultrasonic::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();
}

void Handlers::setName(BLECharacteristic &characteristic, uint8_t *packet)
{
  unsigned char packetLength = packet[1];
  unsigned char nameLength = packetLength - 4;
  char name[17];
  for (int i = 0; i <= nameLength; i++)
  {
    if (i == nameLength)
    {
      name[i] = '\0';
    }
    else
    {
      name[i] = packet[i + 3];
    }
  }
  BLEManager::updateDeviceName(name);
}

void Handlers::motorMoveControl(BLECharacteristic &characteristic, uint8_t *packet)
{
  uint8_t direction = packet[3];
  uint8_t speed = packet[4];
  Serial.println("CMD_MOTOR_MOVE_CONTROL, direction: " + String(direction) + ", speed: " + String(speed));
  // 移动
  if (direction == 0x00)
  {
    MotorController::motorControl('A', 0);
    MotorController::motorControl('B', 0);
    MotorController::motorControl('C', 0);
    MotorController::motorControl('D', 0);
  }
  // 前进
  else if (direction == 0x01)
  {
    MotorController::motorControl('A', speed);
    MotorController::motorControl('B', speed);
    MotorController::motorControl('C', speed);
    MotorController::motorControl('D', speed);
  }
  // 后退
  else if (direction == 0x02)
  {
    MotorController::motorControl('A', -speed);
    MotorController::motorControl('B', -speed);
    MotorController::motorControl('C', -speed);
    MotorController::motorControl('D', -speed);
  }
}

void Handlers::motorSteerControl(BLECharacteristic &characteristic, uint8_t *packet)
{
  uint8_t direction = packet[3];
  uint8_t diffSpeed = packet[4];
  Serial.println("CMD_MOTOR_STEER_CONTROL, direction: " + String(direction) + ", diffSpeed: " + String(diffSpeed));
  // 左转
  if (direction == 0x00)
  {
    MotorController::motorControl('A', MotorController::getMotorStatus('A').pwm - diffSpeed);
    MotorController::motorControl('B', MotorController::getMotorStatus('B').pwm - diffSpeed);
  }
  // 右转
  else if (direction == 0x01)
  {
    MotorController::motorControl('C', MotorController::getMotorStatus('C').pwm - diffSpeed);
    MotorController::motorControl('D', MotorController::getMotorStatus('D').pwm - diffSpeed);
  }
}

void Handlers::motorRotateControl(BLECharacteristic &characteristic, uint8_t *packet)
{
  uint8_t direction = packet[3];
  uint8_t time = packet[4];
  Serial.println("CMD_MOTOR_ROTATE_CONTROL, direction: " + String(direction) + ", time: " + String(time));
  // 顺时针
  if (direction == 0x00)
  {
    MotorController::motorControl('A', 255);
    MotorController::motorControl('B', 255);
    MotorController::motorControl('C', -255);
    MotorController::motorControl('D', -255);
  }
  // 逆时针
  else if (direction == 0x01)
  {
    MotorController::motorControl('A', -255);
    MotorController::motorControl('B', -255);
    MotorController::motorControl('C', 255);
    MotorController::motorControl('D', 255);
  }
}

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

void Handlers::motorXYRControl(BLECharacteristic &characteristic, uint8_t *packet)
{
  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);
  MotorController::motorXYRControl(x, y, r);
}

void Handlers::demoPID(BLECharacteristic &characteristic, uint8_t *packet)
{
  Serial.println("CMD_DEMO_PID");

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

  float targetSpeed = 255;
  float currentSpeed = 0;
  float lastError = 0;
  float integral = 0;
  unsigned long 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;

    // 应用到所有电机
    MotorController::motorControl('A', output);
    MotorController::motorControl('B', output);
    MotorController::motorControl('C', output);
    MotorController::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;

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

    lastError = error;
    delay(10);
  }

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