/**
 * @file CanBusTTL.cpp
 * @brief 使用CAN转TTL模块的CAN通信实现
 * 
 * 本文件暂时保留作为参考
 * 
 * 接线说明(CAN转TTL模块):
 * - ESP32 GPIO17 (TX) → 模块 RXD
 * - ESP32 GPIO18 (RX) → 模块 TXD
 * - ESP32 3.3V       → 模块 VCC
 * - ESP32 GND        → 模块 GND
 * 
 * CAN总线端:
 * - 模块 CANH → USB-CAN适配器 CANH
 * - 模块 CANL → USB-CAN适配器 CANL
 * - 需要在总线两端并联120Ω终端电阻
 */

#include "CanBusTTL.h"

CanBusTTL::CanBusTTL() {}

bool CanBusTTL::begin(CanRxCallback callback)
{
    _callback = callback;
    
    /**
     * ESP32-S3 Serial1默认引脚:
     * - TX: GPIO16
     * - RX: GPIO17
     * 
     * 如需更换引脚，可使用:
     * Serial1.begin(baud, SERIAL_8N1, rxPin, txPin);
     */
    Serial1.begin(CAN_TTL_UART_SPEED, SERIAL_8N1, 17, 16);
    
    initialized = true;
    Serial.println("CAN-TTL模块初始化完成 (115200bps)");
    return true;
}

void CanBusTTL::update()
{
    while (Serial1.available()) {
        uint8_t c = Serial1.read();
        if (rx_index < 64) {
            rx_buffer[rx_index++] = c;
        } else {
            rx_index = 0;
        }
        
        if (c == '\n' || rx_index >= 20) {
            if (rx_index >= 5 && _callback) {
                uint16_t canId = (rx_buffer[1] << 8) | rx_buffer[0];
                uint8_t len = rx_buffer[2] > 8 ? 8 : rx_buffer[2];
                _callback(canId, &rx_buffer[3], len);
            }
            rx_index = 0;
        }
    }
}

bool CanBusTTL::sendFrame(uint16_t canId, const uint8_t* data, uint8_t len)
{
    if (!initialized || len > 8) return false;
    
    uint8_t frame[16];
    frame[0] = canId & 0xFF;
    frame[1] = (canId >> 8) & 0xFF;
    frame[2] = len;
    memcpy(&frame[3], data, len);
    frame[3 + len] = '\n';
    
    return Serial1.write(frame, 4 + len) == (4 + len);
}

bool CanBusTTL::sendPoseData(float voltage, int16_t angleZ, int16_t linearX, int16_t linearY, int16_t angularZ)
{
    uint8_t data[8];
    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);
}

bool CanBusTTL::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);
}

bool CanBusTTL::sendEncoderTicks(int32_t tick1, int32_t tick2, int32_t tick3, int32_t tick4)
{
    uint8_t data1[8], data2[8];
    
    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);
    
    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);
}