#include "module/aimbot.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"
#include <string.h>

/* =====================================================================
 * FDCAN 帧索引（反馈方向：下层板?上层板）
 * 注：上层板使用自己的IMU，下层板只发送射击数据与模式
 * ===================================================================== */
#define FB_FRAME_DATA  0u  /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B)      */

/* =====================================================================
 * UART 通信接口（上层板 �?上位�?PC�?
 * ===================================================================== */

int8_t Aimbot_AIStartRecv(Aimbot_AI_t *ai) {
  if (BSP_UART_Receive(BSP_UART_AI, (uint8_t *)ai, sizeof(*ai), true) == DEVICE_OK) {
    return DEVICE_OK;
  }
  return DEVICE_ERR;
}

int8_t Aimbot_AIGetResult(Aimbot_AI_t *ai, Aimbot_AIResult_t *result) {
  if (ai->head[0] != 'M' || ai->head[1] != 'R') {
    return DEVICE_ERR;
  }
  if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) {
    return DEVICE_ERR;
  }
  result->mode                  = ai->mode;
  result->gimbal_t.setpoint.yaw = ai->yaw;
  result->gimbal_t.vel.yaw      = ai->yaw_vel;
  result->gimbal_t.accl.yaw     = ai->yaw_acc;
  result->gimbal_t.setpoint.pit = ai->pitch;
  result->gimbal_t.vel.pit      = ai->pitch_vel;
  result->gimbal_t.accl.pit     = ai->pitch_acc;
  return DEVICE_OK;
}

/**
 * @brief 打包 MCU 数据（UART 发给上位机格式），修正了原始实现中的字段错误�?
 *        四元数来�?quat 参数；欧拉角、角速度来自 gimbal_fb->imu�?
 */
int8_t Aimbot_MCUPack(Aimbot_MCU_t *mcu, const Gimbal_Feedback_t *gimbal_fb,
                      const AHRS_Quaternion_t *quat,
                      float bullet_speed, uint16_t bullet_count, uint8_t mode) {
  if (mcu == NULL || gimbal_fb == NULL || quat == NULL) {
    return DEVICE_ERR_NULL;
  }
  mcu->head[0]       = 'M';
  mcu->head[1]       = 'R';
  mcu->mode          = mode;
  mcu->q[0]          = quat->q0;
  mcu->q[1]          = quat->q1;
  mcu->q[2]          = quat->q2;
  mcu->q[3]          = quat->q3;
  mcu->yaw           = gimbal_fb->imu.eulr.yaw;
  mcu->yaw_vel       = gimbal_fb->imu.gyro.z;
  mcu->pitch         = gimbal_fb->imu.eulr.pit;
  mcu->pitch_vel     = gimbal_fb->imu.gyro.x;
  mcu->bullet_speed  = bullet_speed;
  mcu->bullet_count  = bullet_count;
  mcu->crc16 = CRC16_Calc((const uint8_t *)mcu,
                           sizeof(*mcu) - sizeof(uint16_t), CRC16_INIT);
  if (!CRC16_Verify((const uint8_t *)mcu, sizeof(*mcu))) {
    return DEVICE_ERR;
  }
  return DEVICE_OK;
}

int8_t Aimbot_MCUStartSend(Aimbot_MCU_t *mcu) {
  if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)mcu, sizeof(*mcu), true) == DEVICE_OK) {
    return DEVICE_OK;
  }
  return DEVICE_ERR;
}

/* =====================================================================
 * CAN 通信接口（下层板 �?上层板）
 * ===================================================================== */

/**
 * @brief 初始�?Aimbot CAN 通信：注册指令接收队列和反馈收发队列�?
 *        下层板只需注册 cmd_id；上层板只需注册 fb_base_id �?6 �?ID�?
 *        本函数同时注册两侧所需 ID，上/下层板共用同一初始化流程即可�?
 */
int8_t Aimbot_Init(Aimbot_Param_t *param) {
  if (param == NULL) return DEVICE_ERR_NULL;

  BSP_FDCAN_Init();

  /* 注册 AI 指令帧队列（下层板接收／上层板发送） */
  BSP_FDCAN_RegisterId(param->can, param->cmd_id,
                     BSP_FDCAN_DEFAULT_QUEUE_SIZE);

  /* 注册反馈数据帧队列（上层板接收／下层板发送） */
  for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
    BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,
                       BSP_FDCAN_DEFAULT_QUEUE_SIZE);
  }

  return DEVICE_OK;
}

/**
 * @brief �?Gimbal/IMU/Shoot 数据打包 CAN 反馈结构体�?
/**
 * @brief 【下层板】打包 CAN 反馈结构体。
 */
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
                            float bullet_speed, uint16_t bullet_count,
                            uint8_t mode) {
  if (fb == NULL) {
    return DEVICE_ERR_NULL;
  }
  fb->mode         = mode;
  fb->bullet_speed = bullet_speed;
  fb->bullet_count = bullet_count;
  return DEVICE_OK;
}

/**
 * @brief 【下层板】将反馈数据打成 1 个 CAN 标准帧发给上层板。
 *
 * 帧格式（每帧 8 字节）：
 *   帧0: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B)
 */
void Aimbot_SendFeedbackOnCAN(const Aimbot_Param_t *param,
                               const Aimbot_FeedbackData_t *fb) {
  if (param == NULL || fb == NULL) return;

  BSP_FDCAN_StdDataFrame_t frame;
  frame.dlc = AIMBOT_CAN_DLC;

  /* 帧0: 弹速 + 弹数 + 模式 */
  frame.id = param->fb_base_id + FB_FRAME_DATA;
  memcpy(&frame.data[0], &fb->bullet_speed, 4);
  frame.data[4] = (uint8_t)(fb->bullet_count & 0xFFu);
  frame.data[5] = (uint8_t)((fb->bullet_count >> 8u) & 0xFFu);
  frame.data[6] = fb->mode;
  frame.data[7] = 0u;
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
}

/**
 * @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来�?AI 指令并解析�?
 *
 * 指令帧格式（8 字节，与 vision_bridge 一致）�?
 *   data[0]      : mode (1B)
 *   data[1..3.5] : yaw  (28bit 有符号定点数�?.1µrad/LSB)
 *   data[4.5..7] : pit  (28bit 有符号定点数�?.1µrad/LSB)
 *
 * @return DEVICE_OK  成功解析到新指令
 *         DEVICE_ERR 队列空，无新数据
 */
int8_t Aimbot_ParseCmdFromCAN(const Aimbot_Param_t *param,
                               Aimbot_AIResult_t *result) {
  if (param == NULL || result == NULL) return DEVICE_ERR_NULL;

  BSP_FDCAN_Message_t msg;
  int8_t ret = DEVICE_ERR;
  
  if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg,
                          BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    result->mode = msg.data[0];

    /* 解析 yaw（高 28 位），符号扩展为 int32 */
    int32_t yaw_raw = (int32_t)(((uint32_t)msg.data[1] << 20u) |
                                 ((uint32_t)msg.data[2] << 12u) |
                                 ((uint32_t)msg.data[3] << 4u)  |
                                 ((uint32_t)(msg.data[4] >> 4u) & 0xFu));
    if (yaw_raw & 0x08000000) yaw_raw |= (int32_t)0xF0000000;
    result->gimbal_t.setpoint.yaw = (float)yaw_raw / AIMBOT_ANGLE_SCALE;

    /* 解析 pit（低 28 位），符号扩展为 int32 */
    int32_t pit_raw = (int32_t)(((uint32_t)(msg.data[4] & 0xFu) << 24u) |
                                 ((uint32_t)msg.data[5] << 16u) |
                                 ((uint32_t)msg.data[6] << 8u)  |
                                 (uint32_t)msg.data[7]);
    if (pit_raw & 0x08000000) pit_raw |= (int32_t)0xF0000000;
    result->gimbal_t.setpoint.pit = (float)pit_raw / AIMBOT_ANGLE_SCALE;
    ret = DEVICE_OK;
  }

  if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 1, &msg,
                          BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&result->gimbal_t.vel.yaw, &msg.data[0], 4);
    memcpy(&result->gimbal_t.vel.pit, &msg.data[4], 4);
    ret = DEVICE_OK;
  }

  if (BSP_FDCAN_GetMessage(param->can, param->cmd_id + 2, &msg,
                          BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&result->gimbal_t.accl.yaw, &msg.data[0], 4);
    memcpy(&result->gimbal_t.accl.pit, &msg.data[4], 4);
    ret = DEVICE_OK;
  }

  return ret;
}

/**
 * @brief 【上层板】将 AI 指令通过 CAN 发送给下层板（单帧 8 字节）�?
 *
 * �?vision_bridge.c �?AI_SendCmdOnFDCAN 编码格式完全一致，
 * 上层板可直接调用本函数替�?vision_bridge 中的同名函数�?
 */
void Aimbot_SendCmdOnCAN(const Aimbot_Param_t *param,
                          const Aimbot_AIResult_t *cmd) {
  if (param == NULL || cmd == NULL) return;

  const int32_t yaw = (int32_t)(cmd->gimbal_t.setpoint.yaw * AIMBOT_ANGLE_SCALE);
  const int32_t pit = (int32_t)(cmd->gimbal_t.setpoint.pit * AIMBOT_ANGLE_SCALE);

  BSP_FDCAN_StdDataFrame_t frame = {0};
  frame.id  = param->cmd_id;
  frame.dlc = AIMBOT_CAN_DLC;

  frame.data[0] = cmd->mode;
  frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
  frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
  frame.data[3] = (uint8_t)((yaw >>  4) & 0xFF);
  frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
  frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
  frame.data[6] = (uint8_t)((pit >>  8) & 0xFF);
  frame.data[7] = (uint8_t)(pit         & 0xFF);

  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

    /* 第二帧：发速度 */
  frame.id = param->cmd_id + 1;
  memcpy(&frame.data[0], &cmd->gimbal_t.vel.yaw, 4);
  memcpy(&frame.data[4], &cmd->gimbal_t.vel.pit, 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

  /* 第三帧：发加速度 */
  frame.id = param->cmd_id + 2;
  memcpy(&frame.data[0], &cmd->gimbal_t.accl.yaw, 4);
  memcpy(&frame.data[4], &cmd->gimbal_t.accl.pit, 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
}

/**
 * @brief 【上层板】从 CAN 队列中非阻塞地解析下层板发来的反馈数据（共1帧）。
 *
 * @return DEVICE_OK
 */
int8_t Aimbot_ParseFeedbackFromCAN(const Aimbot_Param_t *param,
                                    Aimbot_FeedbackData_t *fb) {
  if (param == NULL || fb == NULL) return DEVICE_ERR_NULL;

  BSP_FDCAN_Message_t msg;

  /* 帧0: 弹速 + 弹数 + 模式 */
  if (BSP_FDCAN_GetMessage(param->can,
                          param->fb_base_id + FB_FRAME_DATA,
                          &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->bullet_speed, &msg.data[0], 4);
    fb->bullet_count = (uint16_t)(msg.data[4] | ((uint16_t)msg.data[5] << 8u));
    fb->mode         = msg.data[6];
  }

  return DEVICE_OK;
}