#include "can_driver.h"

#include <string.h>

extern CAN_HandleTypeDef hcan1; 
M2006_Motor_t Motor[MOTOR_NUM];
/**
 * @brief  CAN 初始化，滤波器，启动 CAN 并激活中断
 */
void CAN_Driver_Init(void)
{   
    // 启动 CAN
      HAL_CAN_Start(&hcan1); 
    
    // M2006 电机 CAN 滤波器配置
    CAN_FilterTypeDef sFilterConfig;
    sFilterConfig.FilterBank = 0;
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;

    // 配置 ID 和 Mask (接收 0x201 - 0x208)
    sFilterConfig.FilterIdHigh = 0x0402;
    sFilterConfig.FilterIdLow = 0x0000;
    sFilterConfig.FilterMaskIdHigh = 0x040F;
    sFilterConfig.FilterMaskIdLow = 0x0000;
    sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
    sFilterConfig.FilterActivation = ENABLE;
    

    // 激活 CAN 接收 FIFO0 的中断
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); 
}
/**
 * @brief  CAN 发送函数
 * @param  std_id: 标准 ID
 * @param  TxData: 8 字节发送数据
 * @param  len: 数据长度 (对于 M2006 为 8)
 */
void CAN_Driver_Transmit(uint16_t std_id, uint8_t TxData[], uint8_t len) 
{
    CAN_TxHeaderTypeDef TxHeader;
    uint32_t TxMailbox;

    TxHeader.StdId = std_id;
    TxHeader.RTR = CAN_RTR_DATA;
    TxHeader.IDE = CAN_ID_STD;
    TxHeader.DLC = len;

    HAL_CAN_AddTxMessage(&hcan1, &TxHeader, TxData, &TxMailbox);
}

void Motor_Control_Init(void) 
{
    memset(Motor, 0, sizeof(Motor));//Motor各位置全部清零
	  Motor[0].Target_RPM = 0.0f; //第一个电机目标转速设0，确保开始时为静止   
}

void CAN_Rx_Callback(uint16_t std_id, uint8_t RxData[]) 
{
    // M2006 反馈 ID 范围: 0x201 到 0x204 (对应 ID 1-4)
    if (std_id >= 0x201 && std_id <= (0x201 + MOTOR_NUM - 1)) 
   {
        
        uint8_t motor_index = std_id - 0x201; 
        
        if (motor_index < MOTOR_NUM) 
				{
            M2006_Motor_t *m = &Motor[motor_index];
            // 解析数据 (大端序)
            m->rotor_angle    = (uint16_t)(RxData[0] << 8 | RxData[1]);
            m->speed_rpm      = (int16_t)(RxData[2] << 8 | RxData[3]);
            m->actual_current = (int16_t)(RxData[4] << 8 | RxData[5]);
            m->temp           = RxData[6];
        }
    }
}

void Motor_Control_Loop(void) 
{
    uint8_t TxData[8];
    int16_t Motor1_Target_Current = 1000; 
    uint8_t len = 8; // 数据长度固定为 8
	
	
    Motor[1].target_current = Motor1_Target_Current ;
    // 电机 1: 1000 (0x03E8) 
   TxData[0] = (uint8_t)(Motor[1].target_current >> 8);  // 高 8 位: 0x03
   TxData[1] = (uint8_t)(Motor[1].target_current & 0xFF); // 低 8 位: 0xE8
	
	
   TxData[2] = 0;
   TxData[3] = 0;
   TxData[4] = 0;
   TxData[5] = 0;
   TxData[6] = 0;
   TxData[7] = 0;
    	
       
        
  
    // 3. 调用 CAN 驱动发送控制指令
    CAN_Driver_Transmit(0x200, TxData, 8);
}