/**
 * @file CanBus.cpp
 * @brief CAN总线通信实现
 */

#include "CanBus.h"

/**
 * @brief 构造函数
 */
CanBus::CanBus() {}

/**
 * @brief 初始化CAN总线
 * 
 * 初始化流程:
 * 1. 初始化SPI通信
 * 2. 创建MCP2515实例
 * 3. 重置CAN控制器
 * 4. 设置波特率500Kbps 
 * 5. 设置为Normal模式
 * 
 * @param callback CAN接收回调函数
 * @return true: 初始化成功
 */
bool CanBus::begin(CanRxCallback callback)
{
    _callback = callback;
    
    /**
     * SPI初始化参数(使用GPIO5/6/7):
     * - SCK:  GPIO5
     * - MOSI: GPIO6
     * - MISO: GPIO7
     * - CS:   GPIO15 (由CAN_CS_PIN定义)
     */
    SPI.begin(5, 7, 6, CAN_CS_PIN);
    
    mcp2515 = new MCP2515(CAN_CS_PIN);
    mcp2515->reset();
    
    mcp2515->setBitrate(CAN_500KBPS, MCP_8MHZ);
    mcp2515->setNormalMode();
    
    initialized = true;
    Serial.println("CAN初始化完成 (500Kbps)");
    return true;
}

/**
 * @brief 更新CAN总线状态
 * 
 * 采用轮询方式检查接收缓冲区:
 * - 调用mcp2515->readMessage()读取FIFO中的CAN帧
 * - 如果收到有效帧,调用用户注册的回调函数处理
 * 
 * @note 此函数应被loop()或定时任务周期性调用(建议10-50ms间隔)
 */
void CanBus::update()
{
    struct can_frame canMsg;
    
    /**
     * MCP2515::readMessage() 返回值:
     * - MCP2515::ERROR_OK: 成功读取一帧
     * - 其他: 无数据或错误
     * 
     * CAN_EFF_MASK = 0x1FFFFFFF (扩展帧29位ID掩码)
     * 用于提取有效的CAN ID
     */
    if (mcp2515->readMessage(&canMsg) == MCP2515::ERROR_OK && _callback) {
        _callback(canMsg.can_id & CAN_EFF_MASK, canMsg.data, canMsg.can_dlc);
    }
}

/**
 * @brief 发送原始CAN帧
 * 
 * @param canId 11位标准ID或29位扩展ID
 * @param data 数据缓冲区(最大8字节)
 * @param len 数据长度(1-8)
 * @return true: 发送成功, false: 失败
 */
bool CanBus::sendFrame(uint16_t canId, const uint8_t* data, uint8_t len)
{
    if (!initialized || len > 8) return false;
    
    struct can_frame txMsg;
    txMsg.can_id = canId;
    txMsg.can_dlc = len;  // DLC: Data Length Code (0-8)
    memcpy(txMsg.data, data, len);
    
    return mcp2515->sendMessage(&txMsg) == MCP2515::ERROR_OK;
}

/**
 * @brief 发送姿态数据帧
 * 
 * 数据包格式 (8字节):
 * | Voltage | AngleZ | LinearX | LinearY |
 * | 2字节   | 2字节  | 2字节   | 2字节   |
 * 
 * @param voltage 电池电压(V), 内部乘100转为uint16
 * @param angleZ 航向角Z(度)
 * @param linearX X轴线速度(mm/s)
 * @param linearY Y轴线速度(mm/s)
 * @param angularZ 角速度(deg/s), 注: 此参数暂未使用
 */
bool CanBus::sendPoseData(float voltage, int16_t angleZ, int16_t linearX, int16_t linearY, int16_t angularZ)
{
    uint8_t data[8];
    
    /**
     * 小端序(Little Endian)数据打包:
     * 低字节在前,高字节在后
     * 例: 0x1234 -> data[0]=0x34, data[1]=0x12
     */
    uint16_t voltageInt = (uint16_t)(voltage * 100);
    data[0] = voltageInt & 0xFF;
    data[1] = (voltageInt >> 8) & 0xFF;
    data[2] = angleZ & 0xFF;
    data[3] = (angleZ >> 8) & 0xFF;
    data[4] = linearX & 0xFF;
    data[5] = (linearX >> 8) & 0xFF;
    data[6] = linearY & 0xFF;
    data[7] = (linearY >> 8) & 0xFF;
    
    return sendFrame(CAN_UPLINK_BASE, data, 8);
}

/**
 * @brief 发送电机转速数据帧
 * 
 * 数据包格式 (6字节):
 * | RPM1 | RPM2 | RPM3 |
 * | 2字节 | 2字节 | 2字节 |
 * 
 * @param rpm1~rpm3 电机1-3的转速(RPM)
 * @param rpm4 电机4转速(RPM) - 注: 此参数暂未使用
 */
bool CanBus::sendMotorRpm(int16_t rpm1, int16_t rpm2, int16_t rpm3)
{
    uint8_t data[8];
    data[0] = rpm1 & 0xFF;
    data[1] = (rpm1 >> 8) & 0xFF;
    data[2] = rpm2 & 0xFF;
    data[3] = (rpm2 >> 8) & 0xFF;
    data[4] = rpm3 & 0xFF;
    data[5] = (rpm3 >> 8) & 0xFF;
    
    return sendFrame(CAN_UPLINK_BASE + 1, data, 6);
}

/**
 * @brief 发送编码器累计值帧
 * 
 * 由于int32(4字节)数据超出CAN单帧8字节限制,
 * 使用两帧发送4个编码器的数据:
 * - 0x103: 编码器1-2
 * - 0x104: 编码器3-4
 * 
 * @param tick1~tick4 4个编码器的累计脉冲数
 */
bool CanBus::sendEncoderTicks(int32_t tick1, int32_t tick2, int32_t tick3, int32_t tick4)
{
    uint8_t data1[8], data2[8];
    
    // 编码器1-2 -> 0x103
    data1[0] = (uint8_t)(tick1 & 0xFF);
    data1[1] = (uint8_t)((tick1 >> 8) & 0xFF);
    data1[2] = (uint8_t)((tick1 >> 16) & 0xFF);
    data1[3] = (uint8_t)((tick1 >> 24) & 0xFF);
    data1[4] = (uint8_t)(tick2 & 0xFF);
    data1[5] = (uint8_t)((tick2 >> 8) & 0xFF);
    data1[6] = (uint8_t)((tick2 >> 16) & 0xFF);
    data1[7] = (uint8_t)((tick2 >> 24) & 0xFF);
    
    // 编码器3-4 -> 0x104
    data2[0] = (uint8_t)(tick3 & 0xFF);
    data2[1] = (uint8_t)((tick3 >> 8) & 0xFF);
    data2[2] = (uint8_t)((tick3 >> 16) & 0xFF);
    data2[3] = (uint8_t)((tick3 >> 24) & 0xFF);
    data2[4] = (uint8_t)(tick4 & 0xFF);
    data2[5] = (uint8_t)((tick4 >> 8) & 0xFF);
    data2[6] = (uint8_t)((tick4 >> 16) & 0xFF);
    data2[7] = (uint8_t)((tick4 >> 24) & 0xFF);
    
    // 发送两帧,返回第二帧的发送状态
    sendFrame(CAN_UPLINK_BASE + 2, data1, 8);
    return sendFrame(CAN_UPLINK_BASE + 3, data2, 8);
}