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

#define AI_CMD_CAN_DLC       (8u)  /* 1字节mode + 3.5字节yaw + 3.5字节pit */
#define AI_CMD_ANGLE_SCALE   (10000000.0f)  /* 0.0000001 rad per LSB */

int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
  if (ai == NULL) return DEVICE_ERR_NULL;
  
  BSP_FDCAN_Init();
  
	memset(ai, 0, sizeof(AI_t));

	ai->param = param;
	BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);

  return 0;
}

int8_t AI_StartReceiving(AI_t *ai) {
  if (BSP_UART_Receive(BSP_UART_AI,(uint8_t *)&ai->fromhost,sizeof(ai->fromhost), true)==HAL_OK) {
    return DEVICE_OK;}
  return DEVICE_ERR;
}

int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
		ai->tohost.head[0]='M';
		ai->tohost.head[1]='R';	
		ai->tohost.mode=raw_data->mode;
		ai->tohost.pitch=raw_data->pitch;
		ai->tohost.yaw=raw_data->yaw;	
		ai->tohost.pitch_vel=raw_data->pitch_vel;
		ai->tohost.yaw_vel=raw_data->yaw_vel;	
		ai->tohost.q[0]=raw_data->q[0];	
		ai->tohost.q[1]=raw_data->q[1];	
		ai->tohost.q[2]=raw_data->q[2];	
		ai->tohost.q[3]=raw_data->q[3];		
		ai->tohost.bullet_count=10;
		ai->tohost.bullet_speed=10.5;	
		
		ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
		if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
				  return DEVICE_ERR;
		}
		      return DEVICE_OK;
}

int8_t AI_StartSend(AI_t *ai) {
      if (BSP_UART_Transmit(BSP_UART_AI,(uint8_t *)&ai->tohost,sizeof(ai->tohost), true)==HAL_OK)  
      return DEVICE_OK;
}

int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
		if(ai->fromhost.head[0]!='M'&&ai->fromhost.head[1]!='R'){
					return DEVICE_ERR;
		}
//		CRC16_Calc(&ai->fromhost,sizeof(ai->fromhost),ai->fromhost.crc16);
		if(CRC16_Verify((const uint8_t*)&(ai->fromhost), sizeof(ai->fromhost))!=true){
				  return DEVICE_ERR;
		}
		outcmd->gimbal.setpoint.pit	=	ai->fromhost.pitch;
		outcmd->gimbal.setpoint.yaw	=	ai->fromhost.yaw;
		outcmd->mode								=	ai->fromhost.mode;
		outcmd->gimbal.accl.pit			=	ai->fromhost.pitch_acc;
		outcmd->gimbal.vel.pit			=	ai->fromhost.pitch_vel;
		outcmd->gimbal.accl.yaw			=	ai->fromhost.yaw_acc;
		outcmd->gimbal.vel.yaw			=	ai->fromhost.yaw_vel;		
  return DEVICE_OK;
}


/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
  if (cmd == NULL) return;

  /* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
  const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
  const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);

  BSP_FDCAN_StdDataFrame_t frame = {0};
  frame.id = ai->param->vision_id;
  frame.dlc = AI_CMD_CAN_DLC;
  
  /* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
  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);
  
  (void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
}

/* 从CAN消息解析AI命令 (mode + yaw + pit) */
 void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {

  if (cmd == NULL) {
    return;
  }
		BSP_FDCAN_Message_t msg;
  if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
      return;
  }

  /* 解析mode (1字节) */
  cmd->mode = msg.data[0];
  
  /* 解析yaw (3.5字节) */
  int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
  if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
  cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
  
  /* 解析pit (3.5字节) */
  int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
  if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
  cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
  
  /* 其他字段根据需要可以初始化为0 */
  cmd->gimbal.vel.yaw = 0.0f;
  cmd->gimbal.vel.pit = 0.0f;
  cmd->gimbal.accl.yaw = 0.0f;
  cmd->gimbal.accl.pit = 0.0f;
}