#include "transmit.h"

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

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

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, characteristic);
        }
      }
    }
    else
    {
      isReceivingPacket = false;
    }
  }
}

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

  uint8_t cmd = packet[2];

  switch (cmd)
  {
  case CMD_GET_BT_STATUS:
    Handlers::getBTStatus(characteristic);
    break;

  case CMD_GET_SPIFFS_STATUS:
    Handlers::getSPIFFSStatus(characteristic);
    break;

  case CMD_GET_DISTANCE:
    Handlers::getDistance(characteristic);
    break;

  case CMD_SET_NAME:
    Handlers::setName(characteristic, packet);
    break;

  case CMD_SET_PID:
    Handlers::setPID(characteristic, packet);
    break;

  case CMD_MOTOR_MOVE_CONTROL:
    Handlers::motorMoveControl(characteristic, packet);
    break;

  case CMD_MOTOR_STEER_CONTROL:
    Handlers::motorSteerControl(characteristic, packet);
    break;

  case CMD_MOTOR_ROTATE_CONTROL:
    Handlers::motorRotateControl(characteristic, packet);
    break;

  case CMD_MOTOR_SINGLE_CONTROL:
    Handlers::motorSingleControl(characteristic, packet);
    break;

  case CMD_MOTOR_XYR_CONTROL:
    Handlers::motorXYRControl(characteristic, packet);
    break;

  case CMD_MODE_CONTROL:
    Handlers::modeControl(characteristic, packet);
    break;

  case CMD_DEMO_PID:
    Handlers::demoPID(characteristic, packet);
    break;

  default:
    break;
  }
}

void sendStatus()
{
  unsigned long currentMillis = millis();
  if (currentMillis - lastMotorStatusUpdate >= MOTOR_STATUS_INTERVAL)
  {
    lastMotorStatusUpdate = currentMillis;

    uint8_t buffer[PACKET_MAX_LENGTH];
    uint8_t bufferIndex = 0;
    buffer[bufferIndex++] = PACKET_T_HEAD;
    buffer[bufferIndex++] = 0x0F;
    buffer[bufferIndex++] = CMD_STATUS_MOTOR;
    buffer[bufferIndex++] = Mode::mode;
    buffer[bufferIndex++] = (MotorController::getMotorStatus('A').in2 << 1) | MotorController::getMotorStatus('A').in1;
    buffer[bufferIndex++] = MotorController::getMotorStatus('A').pwm;
    buffer[bufferIndex++] = (MotorController::getMotorStatus('B').in2 << 1) | MotorController::getMotorStatus('B').in1;
    buffer[bufferIndex++] = MotorController::getMotorStatus('B').pwm;
    buffer[bufferIndex++] = (MotorController::getMotorStatus('C').in2 << 1) | MotorController::getMotorStatus('C').in1;
    buffer[bufferIndex++] = MotorController::getMotorStatus('C').pwm;
    buffer[bufferIndex++] = (MotorController::getMotorStatus('D').in2 << 1) | MotorController::getMotorStatus('D').in1;
    buffer[bufferIndex++] = MotorController::getMotorStatus('D').pwm;
    buffer[bufferIndex++] = IR::data.length;
    uint16_t irData = 0;
    for (uint8_t i = 0; i < IR::data.length; i++)
    {
      irData |= (IR::data.data[i] << i);
    }
    buffer[bufferIndex++] = irData;
    buffer[bufferIndex++] = PACKET_T_TAIL;
    BLEManager::pTxCharacteristic->setValue(buffer, bufferIndex);
    BLEManager::pTxCharacteristic->notify();
  }
}