/* =============================================================================
 * c_comm.c - 云台<->底盘 板间通讯(CToC,CAN2,帧 ID 与 legacy 完全一致)
 * 帧:    0x149 云->底 摇杆(4 个大端 u16)  0x189 云->底 控制状态
 *        0x019 底->云 发射机构上电状态
 * 云台板(主机): c_comm_master_send() 每 2ms 发摇杆帧+状态帧;
 *        键鼠在线时按 legacy 规则把 WASD/鼠标合成进摇杆量;
 *        Data[0] 必须实时判定 dr16_offline()(dr16_status 掉线后不会自己清零)
 * 底盘板(从机): c_comm_slave_send() 每 2ms 回传发射机构状态;
 *        中断内解析 0x149/0x189 填充 ctoc_remote(底盘的"虚拟遥控器")
 * ========================================================================== */
#include "c_comm.h"
#include "robot_param.h"
#include "referee.h"
#include <string.h>

volatile ctoc_remote_t ctoc_remote;
volatile uint8_t ctoc_shooter_power = 1;
volatile uint32_t ctoc_reply_last_tick = 0;
volatile uint8_t ctoc_remote_fault = 0;
volatile float ctoc_gimbal_yaw = 0.0f;
volatile uint8_t ctoc_att_ready = 0;
static volatile uint32_t ctoc_att_last_tick = 0;
/* 云台自检下发的编码器方向:+1=tick增向CCW -1=CW 0=未检测(0x1A9 flags) */
volatile int8_t ctoc_encoder_ccw = 0;

/* 0x1A9 帧新鲜判定(两板公共:底盘跟随用它门控,云台留作诊断) */
uint8_t ctoc_att_fresh(void)
{
    return (ctoc_att_last_tick != 0 &&
            HAL_GetTick() - ctoc_att_last_tick < CTOC_OFFLINE_TIME_MS);
}

static void c_comm_pack_uint16(uint8_t data[8], const uint16_t val[4])
{
    for (uint8_t i = 0; i < 4; i++)
    {
        data[2 * i] = (uint8_t)(val[i] >> 8);
        data[2 * i + 1] = (uint8_t)(val[i] & 0xFF);
    }
}

static void c_comm_send_frame(hcan_t * hcan, uint32_t std_id, const uint8_t data[8])
{
    CanCtrlData_s msg;
    msg.hcan = hcan;
    msg.tx_header.StdId = std_id;
    msg.tx_header.IDE = CAN_ID_STD;
    msg.tx_header.RTR = CAN_RTR_DATA;
    msg.tx_header.DLC = 8;
    msg.tx_header.TransmitGlobalTime = DISABLE;
    for (uint8_t i = 0; i < 8; i++)
        msg.tx_data[i] = data[i];
    CAN_SendTxMessage(&msg);
}

#if defined(ROBOT_TYPE_GIMBAL)

/* ============================ 云台板(主机) ============================ */

static void c_comm_rx_0x019(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
    (void)std_id;
    (void)dlc;
    ctoc_shooter_power = rx_data[0];   // 发射机构上电状态
    ctoc_remote_fault = rx_data[1] & 0x01;  // 底盘电机故障位
    ctoc_reply_last_tick = HAL_GetTick();
}

void c_comm_init(void)
{
    can_rx_register(&hcan2, CTOC_MASTER_ID1, c_comm_rx_0x019);
}

uint8_t ctoc_chassis_fault(void)
{
    /* 从未收到过 0x019 视为底盘不在场,不判故障(便于仅云台台架调试);
     * 收到过之后,故障位或帧超时都判故障 */
    if (ctoc_reply_last_tick == 0)
        return 0;
    if (HAL_GetTick() - ctoc_reply_last_tick > CTOC_OFFLINE_TIME_MS)
        return 1;
    return ctoc_remote_fault;
}

/* 云台板:0x1A9 姿态帧 */
void c_comm_master_send_attitude(float yaw_deg, uint8_t flags)
{
    uint8_t data[8] = {0};
    memcpy(data, &yaw_deg, 4);   // float 小端
    data[4] = flags;
    c_comm_send_frame(&hcan2, CTOC_SLAVE_ID3, data);
}

void c_comm_master_send(uint8_t keyboard_online, const uint8_t kb[12], uint8_t motor_fault, uint8_t spin_enable)
{
    uint8_t data[8];

    if (!keyboard_online)
    {
        // 遥控器控制:直接转发原始摇杆值
        const uint16_t sticks[4] = {dr16.rc_r_RL, dr16.rc_r_UD, dr16.rc_l_RL, dr16.rc_l_UD};
        c_comm_pack_uint16(data, sticks);
    }
    else
    {
        // 键鼠控制:合成摇杆量(与 legacy 一致)
        uint8_t key_go = kb[8] & 0x01;
        uint8_t key_back = (kb[8] >> 1) & 0x01;
        uint8_t key_left = (kb[8] >> 2) & 0x01;
        uint8_t key_right = (kb[8] >> 3) & 0x01;
        int16_t mouse_rl = (int16_t)(((uint16_t)kb[1] << 8) | kb[0]);
        int16_t mouse_du = (int16_t)(((uint16_t)kb[3] << 8) | kb[2]);

        const uint16_t sticks[4] = {
            (uint16_t)(1024 + (key_right - key_left) * 660),                 // AD->右摇杆RL
            (uint16_t)(1024 + (key_go - key_back) * 660),                    // WS->右摇杆UD
            (uint16_t)(1024 + mouse_rl * PC_MOUSE_RL_SENSITIVITY),           // 鼠标->左摇杆RL
            (uint16_t)(1024 + mouse_du * PC_MOUSE_DU_SENSITIVITY),           // 鼠标->左摇杆UD
        };
        c_comm_pack_uint16(data, sticks);
    }
    c_comm_send_frame(&hcan2, CTOC_SLAVE_ID1, data);

    /* 在线状态必须实时判定:dr16_status 只在收帧时置 1,掉线后不会自己清零,
     * 若直接转发会让底盘永远认为遥控器在线(停车保护失效) */
    data[0] = (dr16_status && !dr16_offline()) ? 1 : 0;
    data[1] = dr16.s2;
    data[2] = dr16.key_push_ctrl;
    data[3] = dr16.key_push_shift;
    data[4] = dr16_start_flag;
    data[5] = dr16.s1;
    data[6] = (motor_fault ? 0x80 : 0) | (spin_enable ? 0x04 : 0) |
              (dr16.key_alpha1 ? 0x01 : 0) |
              (dr16.key_alpha2 ? 0x02 : 0);  // bit7=故障 bit2=陀螺使能(扩展)
    data[7] = 0;
    c_comm_send_frame(&hcan2, CTOC_SLAVE_ID2, data);
}

/* 0x1B9 云台->底盘:自定义UI 专用量(底盘本地没有,见 referee_ui)
 * [0..1] pitch(0.01°)  [2..3] 左摩擦轮(rpm/100)  [4..5] 右摩擦轮(rpm/100)
 * [6] bit0=小陀螺使能 */
void c_comm_master_send_ui(float pitch_deg, float fric_l_rpm, float fric_r_rpm, uint8_t spin_on)
{
    uint8_t data[8] = {0};
    int16_t p  = (int16_t)(pitch_deg * 100.0f);
    int16_t fl = (int16_t)(fric_l_rpm / 100.0f);
    int16_t fr = (int16_t)(fric_r_rpm / 100.0f);

    data[0] = (uint8_t)(p & 0xFF);
    data[1] = (uint8_t)((uint16_t)p >> 8);
    data[2] = (uint8_t)(fl & 0xFF);
    data[3] = (uint8_t)((uint16_t)fl >> 8);
    data[4] = (uint8_t)(fr & 0xFF);
    data[5] = (uint8_t)((uint16_t)fr >> 8);
    data[6] = spin_on ? 0x01 : 0;
    c_comm_send_frame(&hcan2, CTOC_SLAVE_ID4, data);
}

#elif defined(ROBOT_TYPE_CHASSIS)

/* ============================ 底盘板(从机) ============================ */

static void c_comm_rx_0x149(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
    (void)std_id;
    (void)dlc;
    ctoc_remote.rc_r_rl = ((uint16_t)rx_data[0] << 8) | rx_data[1];
    ctoc_remote.rc_r_ud = ((uint16_t)rx_data[2] << 8) | rx_data[3];
    ctoc_remote.rc_l_rl = ((uint16_t)rx_data[4] << 8) | rx_data[5];
    ctoc_remote.rc_l_ud = ((uint16_t)rx_data[6] << 8) | rx_data[7];
    ctoc_remote.last_tick = HAL_GetTick();
}

static void c_comm_rx_0x189(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
    (void)std_id;
    (void)dlc;
    ctoc_remote.status = rx_data[0];
    ctoc_remote.s2 = rx_data[1];
    ctoc_remote.ctrl_push = rx_data[2];
    ctoc_remote.shift_push = rx_data[3];
    ctoc_remote.start_flag = rx_data[4];
    ctoc_remote.s1 = rx_data[5];
    ctoc_remote.spin_flag = (rx_data[6] >> 2) & 0x01;        // 小陀螺使能位
    ctoc_remote.gimbal_fault = (rx_data[6] & 0x80) ? 1 : 0;  // 云台电机故障位
    ctoc_remote.last_tick = HAL_GetTick();
}

static void c_comm_rx_0x1a9(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
    (void)std_id;
    (void)dlc;
    float yaw;
    memcpy(&yaw, rx_data, 4);
    ctoc_gimbal_yaw = yaw;
    ctoc_att_ready = rx_data[4] & 0x01;
    if (rx_data[4] & 0x04)                                  // 云台自检完成位
        ctoc_encoder_ccw = (rx_data[4] & 0x02) ? 1 : -1;    // tick 增方向
    ctoc_att_last_tick = HAL_GetTick();
}

void c_comm_init(void)
{
    can_rx_register(&hcan2, CTOC_SLAVE_ID1, c_comm_rx_0x149);
    can_rx_register(&hcan2, CTOC_SLAVE_ID2, c_comm_rx_0x189);
    can_rx_register(&hcan2, CTOC_SLAVE_ID3, c_comm_rx_0x1a9);
}

void c_comm_slave_send(uint8_t motor_fault)
{
    uint8_t data[8] = {0};
    data[0] = referee.shooter_power_on;       // 发射机构上电状态
    data[1] = motor_fault ? 0x01 : 0;         // 底盘电机故障位(扩展)
    c_comm_send_frame(&hcan2, CTOC_MASTER_ID1, data);
}

#endif