#include "device/ai.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"
#include "device/referee.h"

/* ---- 接收缓冲 ---- */
static uint8_t ai_rx_buf[64];        /* DMA 接收缓冲，略大于 PackageAI_t */
static AI_result_t ai_parsed;        /* 解析后的结果 */
static volatile uint8_t ai_data_ready = 0;

static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
  uint16_t *crc = NULL;

  if (raw == NULL || len <= sizeof(uint16_t)) {
    return DEVICE_ERR;
  }

  raw[0] = id;
  crc = (uint16_t *)(void *)(raw + len - sizeof(uint16_t));
  *crc = CRC16_Calc(raw, len - sizeof(uint16_t), CRC16_INIT);

  if (CRC16_Verify(raw, len) != true) {
    return DEVICE_ERR;
  }

  return DEVICE_OK;
}

/* IDLE LINE 回调 —— 上位机发完一帧后总线空闲触发 */
static void AI_IdleLineCallback(void) {
  UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);

  /* 停止DMA，计算实际收到的字节数 */
  HAL_UART_DMAStop(huart);
  uint16_t rx_len = sizeof(ai_rx_buf) - __HAL_DMA_GET_COUNTER(huart->hdmarx);

  /* 长度匹配才解析 */
  if (rx_len == sizeof(PackageAI_t)) {
    PackageAI_t *pkt = (PackageAI_t *)ai_rx_buf;

    if (pkt->id == ID_AI && CRC16_Verify((uint8_t *)pkt, sizeof(PackageAI_t))) {
      ai_parsed.mode = pkt->data.mode;
      ai_parsed.gimbal_t.setpoint.yaw = pkt->data.yaw;
      ai_parsed.gimbal_t.vel.yaw     = pkt->data.yaw_vel;
      ai_parsed.gimbal_t.accl.yaw    = pkt->data.yaw_acc;
      ai_parsed.gimbal_t.setpoint.pit = pkt->data.pitch;
      ai_parsed.gimbal_t.vel.pit     = pkt->data.pitch_vel;
      ai_parsed.gimbal_t.accl.pit    = pkt->data.pitch_acc;
      ai_parsed.chassis_t.Vx = pkt->data.vx;
      ai_parsed.chassis_t.Vy = pkt->data.vy;
      ai_parsed.chassis_t.Vw = pkt->data.wz;
      ai_data_ready = 1;
    }
  }

  /* 重启DMA接收，等待下一帧 */
  BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
}

/**
 * @brief 初始化AI通信，注册IDLE回调并启动DMA接收
 */
int8_t AI_Init(void) {
  UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_1);

  /* 注册IDLE LINE回调 */
  BSP_UART_RegisterCallback(BSP_UART_1, BSP_UART_IDLE_LINE_CB,
                            (void (*)(void))AI_IdleLineCallback);

  BSP_UART_RegisterCallback(BSP_UART_1, BSP_UART_IDLE_LINE_CB, (void (*)(void))AI_IdleLineCallback);

  /* 启动第一次DMA接收 */
  BSP_UART_Receive(BSP_UART_1, ai_rx_buf, sizeof(ai_rx_buf), true);
  return DEVICE_OK;
}

/**
 * @brief 获取最新的AI数据（非阻塞，task循环中调用）
 * @return DEVICE_OK=有新数据已拷贝, DEVICE_ERR=无新数据
 */
int8_t AI_GetLatest(AI_result_t *result) {
  if (result == NULL) {
    return DEVICE_ERR;
  }
  if (!ai_data_ready) {
    return DEVICE_ERR;
  }
  *result = ai_parsed;
  ai_data_ready = 0;
  return DEVICE_OK;
}

/* ---- 以下发送函数不变 ---- */

int8_t MCU_Send(PackageMCU_t *mcu, Gimbal_feedback_t *motor, AHRS_Quaternion_t *quat) {
  (void)quat;
  if (mcu == NULL || motor == NULL) {
    return DEVICE_ERR;
  }

  mcu->data.mode = 1;
  mcu->data.q[0] = motor->imu.quat.q0;
  mcu->data.q[1] = motor->imu.quat.q1;
  mcu->data.q[2] = motor->imu.quat.q2;
  mcu->data.q[3] = motor->imu.quat.q3;
  mcu->data.yaw = motor->imu.eulr.yaw;
  mcu->data.yaw_vel = motor->imu.gyro.z;
  mcu->data.pitch = motor->imu.eulr.rol;
  mcu->data.pitch_vel = motor->imu.gyro.x;
  mcu->data.bullet_speed = 22;

  return Package_BuildAndVerify((uint8_t *)mcu, sizeof(*mcu), ID_MCU);
}

int8_t REF_Send(PackageReferee_t *referee, Referee_RobotStatus_t *robot_status, Referee_GameStatus_t *game_status) {
  if (referee == NULL || robot_status == NULL || game_status == NULL) {
    return DEVICE_ERR;
  }

  referee->data.remain_hp = robot_status->current_HP;
  referee->data.game_progress = game_status->game_progress & 0x0F;
  referee->data.stage_remain_time = game_status->stage_remain_time;

  return Package_BuildAndVerify((uint8_t *)referee, sizeof(*referee), ID_REF);
}

int8_t REF_StartSend(PackageReferee_t *referee) {
  if (referee == NULL) {
    return DEVICE_ERR;
  }

  if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)referee, sizeof(*referee), true) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}

int8_t MCU_StartSend(PackageMCU_t *mcu) {
  if (BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}