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

/* =====================================================================
 * CAN 帧索引（反馈方向：下层板 → 上层板）
 * ===================================================================== */
#define FB_FRAME_Q01    0u  /* IMU 四元数 q[0](w), q[1](x)              */
#define FB_FRAME_Q23    1u  /* IMU 四元数 q[2](y), q[3](z)              */
#define FB_FRAME_EULR   2u  /* IMU 欧拉角 yaw, pitch (float x2)         */
#define FB_FRAME_VEL    3u  /* IMU 角速度 yaw_vel, pitch_vel (float x2) */
#define FB_FRAME_SHOOT  4u  /* 弹速(4B)+弹数(2B)+模式(1B)+保留(1B)      */
#define FB_FRAME_MOTOR  5u  /* 云台电机绝对角度 yaw, pit (float x2)     */

/* =====================================================================
 * 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->id != ID_AI) {
    return DEVICE_ERR;
  }
  if (!CRC16_Verify((const uint8_t *)ai, sizeof(*ai))) {
    return DEVICE_ERR;
  }
  result->mode                  = ai->data.mode;
  result->gimbal_t.setpoint.yaw = ai->data.yaw;
  result->gimbal_t.vel.yaw      = ai->data.yaw_vel;
  result->gimbal_t.accl.yaw     = ai->data.yaw_acc;
  result->gimbal_t.setpoint.pit = ai->data.pitch;
  result->gimbal_t.vel.pit      = ai->data.pitch_vel;
  result->gimbal_t.accl.pit     = ai->data.pitch_acc;
  result->chassis_t.Vx          = ai->data.vx;
  result->chassis_t.Vy          = ai->data.vy;
  result->chassis_t.Vw          = ai->data.wz;
  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) {
  if (mcu == NULL || gimbal_fb == NULL || quat == NULL) {
    return DEVICE_ERR_NULL;
  }
  mcu->id           = ID_MCU;
  mcu->data.mode    = 0;
  mcu->data.q[0]    = quat->q0;
  mcu->data.q[1]    = quat->q1;
  mcu->data.q[2]    = quat->q2;
  mcu->data.q[3]    = quat->q3;
  mcu->data.yaw         = gimbal_fb->imu.eulr.yaw;
  mcu->data.yaw_vel     = gimbal_fb->imu.gyro.z;
  mcu->data.pitch       = gimbal_fb->imu.eulr.pit;
  mcu->data.pitch_vel   = gimbal_fb->imu.gyro.x;
  mcu->data.bullet_speed  = bullet_speed;
  mcu->data.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 反馈结构体。
 */
int8_t Aimbot_PackFeedback(Aimbot_FeedbackData_t *fb,
                            const Gimbal_Feedback_t *gimbal_fb,
                            const AHRS_Quaternion_t *quat,
                            float bullet_speed, uint16_t bullet_count,
                            uint8_t mode) {
  if (fb == NULL || gimbal_fb == NULL || quat == NULL) {
    return DEVICE_ERR_NULL;
  }
  fb->mode         = mode;
  fb->q[0]         = quat->q0;
  fb->q[1]         = quat->q1;
  fb->q[2]         = quat->q2;
  fb->q[3]         = quat->q3;
  fb->yaw          = gimbal_fb->imu.eulr.yaw;
  fb->pitch        = gimbal_fb->imu.eulr.pit;
  fb->yaw_vel      = gimbal_fb->imu.gyro.z;
  fb->pitch_vel    = gimbal_fb->imu.gyro.x;
  fb->bullet_speed = bullet_speed;
  fb->bullet_count = bullet_count;
  fb->gimbal_yaw   = gimbal_fb->motor.yaw.rotor_abs_angle;
  fb->gimbal_pit   = gimbal_fb->motor.pit.rotor_abs_angle;
  return DEVICE_OK;
}

/**
 * @brief 【下层板】将反馈数据打成 6 个 CAN 标准帧发给上层板。
 *
 * 帧格式（每帧 8 字节）：
 *   帧0: q[0](float,4B)  q[1](float,4B)
 *   帧1: q[2](float,4B)  q[3](float,4B)
 *   帧2: yaw(float,4B)   pitch(float,4B)
 *   帧3: yaw_vel(float,4B) pitch_vel(float,4B)
 *   帧4: bullet_speed(float,4B) bullet_count(uint16,2B) mode(1B) rsv(1B)
 *   帧5: gimbal_yaw(float,4B)  gimbal_pit(float,4B)
 */
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: IMU 四元数 q[0], q[1] */
  frame.id = param->fb_base_id + FB_FRAME_Q01;
  memcpy(&frame.data[0], &fb->q[0], 4);
  memcpy(&frame.data[4], &fb->q[1], 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

  /* 帧1: IMU 四元数 q[2], q[3] */
  frame.id = param->fb_base_id + FB_FRAME_Q23;
  memcpy(&frame.data[0], &fb->q[2], 4);
  memcpy(&frame.data[4], &fb->q[3], 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

  /* 帧2: IMU 欧拉角 yaw, pitch */
  frame.id = param->fb_base_id + FB_FRAME_EULR;
  memcpy(&frame.data[0], &fb->yaw,   4);
  memcpy(&frame.data[4], &fb->pitch, 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

  /* 帧3: IMU 角速度 yaw_vel, pitch_vel */
  frame.id = param->fb_base_id + FB_FRAME_VEL;
  memcpy(&frame.data[0], &fb->yaw_vel,   4);
  memcpy(&frame.data[4], &fb->pitch_vel, 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);

  /* 帧4: 弹速 + 弹数 + 模式 */
  frame.id = param->fb_base_id + FB_FRAME_SHOOT;
  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);

  /* 帧5: 云台电机绝对角度 yaw, pit */
  frame.id = param->fb_base_id + FB_FRAME_MOTOR;
  memcpy(&frame.data[0], &fb->gimbal_yaw, 4);
  memcpy(&frame.data[4], &fb->gimbal_pit, 4);
  BSP_FDCAN_TransmitStdDataFrame(param->can, &frame);
}

/**
 * @brief 【下层板】从 CAN 队列中非阻塞地取出上层板发来的 AI 指令并解析。
 *
 * 指令帧格式（8 字节，与 vision_bridge 一致）：
 *   data[0]      : mode (1B)
 *   data[1..3.5] : yaw  (28bit 有符号定点数，0.1µrad/LSB)
 *   data[4.5..7] : pit  (28bit 有符号定点数，0.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;
  if (BSP_FDCAN_GetMessage(param->can, param->cmd_id, &msg,
                            BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
    return DEVICE_ERR;
  }

  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;

  /* 速度/加速度前馈由上层板另行扩展，此处置零 */
  result->gimbal_t.vel.yaw  = 0.0f;
  result->gimbal_t.vel.pit  = 0.0f;
  result->gimbal_t.accl.yaw = 0.0f;
  result->gimbal_t.accl.pit = 0.0f;

  return DEVICE_OK;
}

/**
 * @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);
}

/**
 * @brief 【上层板】从 CAN 队列中逐帧非阻塞地解析下层板发来的反馈数据。
 *        各帧独立读取，缺失的帧保留上一次的旧值，不影响其他帧。
 *
 * @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: IMU 四元数 q[0], q[1] */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_Q01,
                            &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->q[0], &msg.data[0], 4);
    memcpy(&fb->q[1], &msg.data[4], 4);
  }

  /* 帧1: IMU 四元数 q[2], q[3] */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_Q23,
                            &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->q[2], &msg.data[0], 4);
    memcpy(&fb->q[3], &msg.data[4], 4);
  }

  /* 帧2: IMU 欧拉角 yaw, pitch */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_EULR,
                            &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->yaw,   &msg.data[0], 4);
    memcpy(&fb->pitch, &msg.data[4], 4);
  }

  /* 帧3: IMU 角速度 yaw_vel, pitch_vel */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_VEL,
                            &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->yaw_vel,   &msg.data[0], 4);
    memcpy(&fb->pitch_vel, &msg.data[4], 4);
  }

  /* 帧4: 弹速 + 弹数 + 模式 */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_SHOOT,
                            &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];
  }

  /* 帧5: 云台电机绝对角度 yaw, pit */
  if (BSP_FDCAN_GetMessage(param->can,
                            param->fb_base_id + FB_FRAME_MOTOR,
                            &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
    memcpy(&fb->gimbal_yaw, &msg.data[0], 4);
    memcpy(&fb->gimbal_pit, &msg.data[4], 4);
  }

  return DEVICE_OK;
}