#include "main.h"
#include "can.h"
#include "gpio.h"
#include "can2.h"
#include "remote.h"

// 遥控器数据CAN发送函数
void send_rc_data_complete(rc_info_t *rc)
{
    static CAN_TxHeaderTypeDef rc_tx_message;
    static uint8_t rc_can_send_data[8];
    static uint8_t initialized = 0;
    uint32_t send_mail_box;
    
    // 初始化CAN消息头（只初始化一次）
    if (!initialized) {
        rc_tx_message.StdId = 0x301;           // 遥控器数据的CAN ID
        rc_tx_message.IDE = CAN_ID_STD;        // 标准ID
        rc_tx_message.RTR = CAN_RTR_DATA;      // 数据帧
        rc_tx_message.DLC = 0x08;              // 8字节数据长度
        initialized = 1;
    }
    
    // 将16位数据拆分为高8位和低8位
    // 通道数据 (-660~660, 实际是-1024~1024)
    rc_can_send_data[0] = (rc->ch0 >> 8) & 0xFF;    // ch0高8位
    rc_can_send_data[1] = rc->ch0 & 0xFF;          // ch0低8位
    rc_can_send_data[2] = (rc->ch1 >> 8) & 0xFF;    // ch1高8位
    rc_can_send_data[3] = rc->ch1 & 0xFF;          // ch1低8位
    rc_can_send_data[4] = (rc->ch2 >> 8) & 0xFF;    // ch2高8位
    rc_can_send_data[5] = rc->ch2 & 0xFF;          // ch2低8位
    
    // ch3和roll只有11位有效，进行优化打包
    // ch3低8位 + 开关位
    rc_can_send_data[6] = (rc->ch3 & 0xFF);        // ch3低8位
    
    // 开关位组合：sw2高2位 + sw1低2位 + ch3的高3位(bit8-10)
    rc_can_send_data[7] = ((rc->ch3 >> 8) & 0x07) |        // ch3高3位
                          ((rc->sw1 & 0x03) << 3) |       // sw1占2位
                          ((rc->sw2 & 0x03) << 5);        // sw2占2位
    
    // 发送CAN数据
    HAL_CAN_AddTxMessage(&hcan2, &rc_tx_message, rc_can_send_data, &send_mail_box);
}

// 如果需要发送roll数据，可以再添加一个函数
void send_rc_roll_data(rc_info_t *rc)
{
    static CAN_TxHeaderTypeDef roll_tx_message;
    static uint8_t roll_can_send_data[8];
    static uint8_t initialized = 0;
    uint32_t send_mail_box;
    
    if (!initialized) {
        roll_tx_message.StdId = 0x302;           // roll数据的CAN ID
        roll_tx_message.IDE = CAN_ID_STD;
        roll_tx_message.RTR = CAN_RTR_DATA;
        roll_tx_message.DLC = 0x02;              // 2字节数据长度
        initialized = 1;
    }
    
    // roll数据 (-660~660)
    roll_can_send_data[0] = (rc->roll >> 8) & 0xFF;  // roll高8位
    roll_can_send_data[1] = rc->roll & 0xFF;         // roll低8位
    
    HAL_CAN_AddTxMessage(&hcan2, &roll_tx_message, roll_can_send_data, &send_mail_box);
}