/* =============================================================================
 * c_comm_task.c - 板间通讯周期发送(2ms)
 * 云台板:  c_comm_master_send() 转发摇杆/控制状态(键鼠在线时用合成量)
 * 底盘板:  c_comm_slave_send() 回传发射机构上电状态
 * 接收方向在中断内由 c_comm 模块的回调完成,本任务只负责发送节拍
 * ========================================================================== */
#include "app_tasks.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "c_comm.h"
#include "robot_typedef.h"
#include "robot_param.h"
#if defined(ROBOT_TYPE_GIMBAL)
#include "robot_cmd_task.h"
#include "gimbal.h"
#include "attitude.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#endif

/* c_comm_task(2ms):板间通讯周期发送
 * 云台板:向底盘转发摇杆与控制状态(键鼠在线时用合成量替代摇杆)
 * 底盘板:向云台回传发射机构上电状态 */

void c_comm_task(void const * argument)
{
    TickType_t last_wake = xTaskGetTickCount();

    while (!app_ready)
        osDelay(2);

    for (;;)
    {
#if defined(ROBOT_TYPE_GIMBAL)
        /* 键鼠来源已由 robot_cmd_task 合并(0x0304 优先,否则 USART3 帧内键鼠)。
         * 直接用共享视图,保证"云台怎么控、底盘就怎么收"——否则 WASD 合成的
         * 底盘摇杆会因为这里只认 0x0304 而被整段跳过(客户端键鼠走帧内时平移失效)。 */
        uint8_t kb_online = keyboard_merged_online;
        uint8_t kb[12];
        kb[0] = (uint8_t)(keyboard_merged.mouse_rl & 0xFF);
        kb[1] = (uint8_t)((uint16_t)keyboard_merged.mouse_rl >> 8);
        kb[2] = (uint8_t)(keyboard_merged.mouse_du & 0xFF);
        kb[3] = (uint8_t)((uint16_t)keyboard_merged.mouse_du >> 8);
        kb[4] = (uint8_t)(keyboard_merged.mouse_wheel & 0xFF);
        kb[5] = (uint8_t)((uint16_t)keyboard_merged.mouse_wheel >> 8);
        kb[6] = keyboard_merged.mouse_key_l;
        kb[7] = keyboard_merged.mouse_key_r;
        kb[8] = (uint8_t)(keyboard_merged.key_w | (keyboard_merged.key_s << 1) |
                          (keyboard_merged.key_a << 2) | (keyboard_merged.key_d << 3) |
                          (keyboard_merged.key_shift << 4) | (keyboard_merged.key_ctrl << 5) |
                          (keyboard_merged.key_q << 6) | (keyboard_merged.key_e << 7));
        kb[9] = (uint8_t)(keyboard_merged.key_alpha1 | (keyboard_merged.key_alpha2 << 1));
        kb[10] = 0;
        kb[11] = 0;

        c_comm_master_send(kb_online, kb, gimbal_motor_offline(),
                           cmd_robot.spin_flag);  // 故障位+小陀螺使能位

        /* 0x1B9 自定义UI 专用量:底盘本地没有 pitch 与摩擦轮转速,
         * 由云台随板间通讯带给它(见 底盘/modules/comms/referee_ui.c)。
         * pitch 取云台 pitch 环反馈 -att_deg_roll;摩擦轮转速取电机反馈 rpm。 */
        c_comm_master_send_ui(-att_deg_roll,
                              (float)motor_friction_l.fdb.rotor_speed,
                              (float)motor_friction_r.fdb.rotor_speed,
                              cmd_robot.spin_flag);

        /* 0x1A9 姿态帧:跟随符号自动综合的数据源
         * flags:bit0=att_ready bit1=tick增向CCW bit2=bit1有效 */
        {
            const int8_t enc = gimbal_encoder_ccw();
            uint8_t flags = att_ready;
            if (enc > 0) flags |= 0x02;
            if (enc != 0) flags |= 0x04;
            c_comm_master_send_attitude(att_deg_yaw, flags);
        }
#elif defined(ROBOT_TYPE_CHASSIS)
        c_comm_slave_send(chassis_motor_offline());  // 携带底盘电机故障位
#endif

        monitor_heartbeat(HB_CCOMM);
        vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
    }
}