/*
  控制命令
*/
#include "module/cmd.h"

#include <stdint.h>
#include <string.h>
#include "bsp/time.h"
#include "device/vt13.h"
#include "module/shoot.h"

/*************************************************************************************************************************************/
/****************************************************************RC*******************************************************************/
/*************************************************************************************************************************************/
/* 扩展接口 */
#if CMD_RCTypeTable_Index == 0
#define CMD_RCType_TABLE(X) X(dr16, DR16)
#elif CMD_RCTypeTable_Index == 1
#define CMD_RCType_TABLE(X) X(at9s, AT9S)
#elif CMD_RCTypeTable_Index == 2
#define CMD_RCType_TABLE(X) X(vt13, VT13)
#endif
/* Includes ----------------------------------------------------------------- */
#if CMD_RCTypeTable_Index == 0
#include "device/dr16.h"
#elif CMD_RCTypeTable_Index == 1
#include "device/at9s_pro.h"
#elif CMD_RCTypeTable_Index == 2
#include "device/vt13.h"
#endif
/* Private macro ------------------------------------------------------------ */
/* 声明外部rc数据变量 */
#define CMDMACRO_EXTERNAL_RCDATA(name, NAME) extern NAME##_t name;
/* 声明CMD结构体的RC数据变量 */
#define CMDMACRO_VAR_RCDATA(name, NAME) NAME##_DataRC_t name;
/* 宏生成Cmd_RC_Get函数内容 */
#define CMDMACRO_Cmd_RC_Get(name, NAME) \
    bool online= name.header.online; \
    rc_buffer.name= name.data.rc; \
    rc->online=online; \
    rc->type = CMD_RCTypeTable_Index;
union CMD_RC_Data_t{
    CMD_RCType_TABLE(CMDMACRO_VAR_RCDATA)
};

CMD_RCType_TABLE(CMDMACRO_EXTERNAL_RCDATA)
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */

/* Private variables -------------------------------------------------------- */
/*静态缓冲区*/
CMD_RC_Data_t rc_buffer;

/* Private function  -------------------------------------------------------- */
int8_t Cmd_RC_Get(CMD_Input_RC_t *rc){
    CMD_RCType_TABLE(CMDMACRO_Cmd_RC_Get);
    return CMD_OK;
}

int8_t Cmd_RC_BuildChassisCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if CMD_RCTypeTable_Index == 0
    switch (c->input.rc.data->dr16.sw_l) {
    case DR16_SW_DOWN:
      c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
      break;
    case DR16_SW_MID:
      c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
      break;
    case DR16_SW_UP:
      c->output.chassis.cmd.mode = CHASSIS_MODE_BREAK;
      break;
    default:
      c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    }
    c->output.chassis.cmd.ctrl_vec.vx = c->input.rc.data->dr16.ch_r_x;
    c->output.chassis.cmd.ctrl_vec.vy = c->input.rc.data->dr16.ch_r_y;
#elif CMD_RCTypeTable_Index == 1
    switch (c->input.rc.data->at9s.key_E) {
    case AT9S_CMD_SW_DOWN:
      c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    case AT9S_CMD_SW_MID:
      c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
      break;
    case AT9S_CMD_SW_UP:
      c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
      break;
    default:
      c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    }
    c->output.chassis.cmd.ctrl_vec.vx = c->input.rc.data->at9s.ch_r_x;
    c->output.chassis.cmd.ctrl_vec.vy = c->input.rc.data->at9s.ch_r_y;
#endif
    return CMD_OK;
}

int8_t Cmd_RC_BuildGimbalCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if CMD_RCTypeTable_Index == 0
    switch (c->input.rc.data->dr16.sw_l) {
    case DR16_SW_DOWN:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = 0.0f;
      c->output.gimbal.cmd.delta_pit = 0.0f;
      break;
    case DR16_SW_MID:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->dr16.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.data->dr16.ch_l_y * 1.5f;
      break;
    case DR16_SW_UP:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->dr16.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.data->dr16.ch_l_y * 1.5f;
      break;
    default:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->output.gimbal.cmd.delta_yaw = 0.0f;
      c->output.gimbal.cmd.delta_pit = 0.0f;
      break;
    }
#elif CMD_RCTypeTable_Index == 1
	switch (c->input.rc.data->at9s.key_G) {
    case AT9S_CMD_SW_DOWN:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->output.gimbal.cmd.delta_yaw = 0.0f;
      c->output.gimbal.cmd.delta_pit = 0.0f;
      break;
    case AT9S_CMD_SW_MID:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->at9s.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.data->at9s.ch_l_y * 1.5f;
      break;
    case AT9S_CMD_SW_UP:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.data->at9s.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.data->at9s.ch_l_y * 1.5f;
      break;
    default:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->output.gimbal.cmd.delta_yaw = 0.0f;
      c->output.gimbal.cmd.delta_pit = 0.0f;
      break;
    }
#endif
    return CMD_OK;
}


int8_t Cmd_RC_BuildShootCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if CMD_RCTypeTable_Index == 0
    c->output.shoot.cmd.online = c->input.rc.online;
    if (c->output.shoot.cmd.online) {
        c->output.shoot.cmd.mode=SHOOT_MODE_SINGLE;
    } else {
        c->output.shoot.cmd.mode=SHOOT_MODE_SAFE;
    }
    switch (c->input.rc.data->dr16.sw_r) {
    case DR16_SW_DOWN:
      c->output.shoot.cmd.ready    = true;
      c->output.shoot.cmd.firecmd  = true;
      break;
    case DR16_SW_MID:
      c->output.shoot.cmd.ready    = true;
      c->output.shoot.cmd.firecmd  = false;
      break;
    case DR16_SW_UP:
      c->output.shoot.cmd.ready    = false;
      c->output.shoot.cmd.firecmd  = false;
      break;
    default:
      c->output.shoot.cmd.ready    = false;
      c->output.shoot.cmd.firecmd  = false;
      break;
    }
#elif CMD_RCTypeTable_Index == 1
	c->output.shoot.cmd.online = c->input.rc.online;
    switch (c->input.rc.data->at9s.key_C) {
    case AT9S_CMD_SW_DOWN:
      c->output.shoot.cmd.ready    = true;
      c->output.shoot.cmd.firecmd  = true;
      break;
    case AT9S_CMD_SW_MID:
      c->output.shoot.cmd.ready    = true;
      c->output.shoot.cmd.firecmd  = false;
      break;
    case AT9S_CMD_SW_UP:
      c->output.shoot.cmd.ready    = false;
      c->output.shoot.cmd.firecmd  = false;
      break;
    default:
      c->output.shoot.cmd.ready    = false;
      c->output.shoot.cmd.firecmd  = false;
      break;
    }
    switch (c->input.rc.data->at9s.key_D) {
    case AT9S_CMD_SW_DOWN:
        c->output.shoot.cmd.mode=SHOOT_MODE_SINGLE;
      break;
    case AT9S_CMD_SW_UP:
        c->output.shoot.cmd.mode=SHOOT_MODE_BURST;
      break;
    default:
        c->output.shoot.cmd.mode=SHOOT_MODE_SAFE;
      break;
    }
#endif
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */


/*************************************************************************************************************************************/
/*****************************************************************PC******************************************************************/
/*************************************************************************************************************************************/
#ifndef CMD_NOPC_FLAG
/* Private macro ------------------------------------------------------------ */
#define CMD_PCBehavior_TABLE(X) \
    X(FORE, CMD_MODULE_CHASSIS)                 \
    X(BACK, CMD_MODULE_CHASSIS)                 \
    X(LEFT, CMD_MODULE_CHASSIS)                 \
    X(RIGHT, CMD_MODULE_CHASSIS)                \
    X(ACCELERATE, CMD_MODULE_CHASSIS)           \
    X(DECELEBRATE, CMD_MODULE_CHASSIS)          \
    X(FIRE, CMD_MODULE_SHOOT)                   \
    X(FIRE_MODE, CMD_MODULE_SHOOT)              \
    X(BUFF, CMD_MODULE_SHOOT)                   \
    X(AUTOAIM, CMD_MODULE_GIMBAL | CMD_MODULE_SHOOT) \
    X(OPENCOVER, CMD_MODULE_SHOOT)              \
    X(ROTOR, CMD_MODULE_CHASSIS)                \
    X(REVTRIG, CMD_MODULE_SHOOT)                \
    X(FOLLOWGIMBAL35, CMD_MODULE_CHASSIS)

/* 行为处理函数声明宏 */
#define CMDMACRO_FOR_DECLARE_BEHAVIOR_HANDLER_FUNCTION(BEHAVIOR, MODULE_MASK) \
    static int8_t Cmd_PC_HandleBehavior##BEHAVIOR(CMD_t *c);
/* 行为处理函数指针数组构建宏 */
#define CMDMACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY(BEHAVIOR, MODULE_MASK) \
    {CMD_BEHAVIOR_##BEHAVIOR, Cmd_PC_HandleBehavior##BEHAVIOR},
/* 行为模块映射表构建宏 */
#define CMDMACRO_FOR_BUILD_MODULE_TABLE(BEHAVIOR, MODULE_MASK) \
    [CMD_BEHAVIOR_##BEHAVIOR] = MODULE_MASK,
/* 行为处理函数声明 */
CMD_PCBehavior_TABLE(CMDMACRO_FOR_DECLARE_BEHAVIOR_HANDLER_FUNCTION)

#define CMDMACRO_VAR_PCDATA(name, NAME) NAME##_DataPC_t name;
/* 宏展开函数内容 */
#define CMDMACRO_Cmd_PC_Get(name, NAME) \
        bool online= name.header.online; \
        pc_buffer.name= name.data.pc; \
        pc->online=online;
#define CMDMACRO_PC_BuildChassisCommandFromInput(name, NAME) \
    c->output.chassis.cmd.ctrl_vec.vx = 0.0f; \
    c->output.chassis.cmd.ctrl_vec.vy = 0.0f; \
    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
        CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
        if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
            if (CMD_PC_IsBehaviorTriggered(c, i)) { \
                behaviorHandlerFuncTable[i].func(c); \
            } \
        } \
    }
#define CMDMACRO_PC_BuildGimbalCommandFromInput(name, NAME) \
    c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; \
    c->output.gimbal.cmd.delta_yaw = (float)c->input.pc.data->name.mouse.x * c->timer.dt * c->params->pc.sensitivity.sens_mouse; \
    c->output.gimbal.cmd.delta_pit = (float)c->input.pc.data->name.mouse.y * c->timer.dt * c->params->pc.sensitivity.sens_mouse; \
    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
        CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
        if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
            if (CMD_PC_IsBehaviorTriggered(c, i)) { \
                behaviorHandlerFuncTable[i].func(c); \
            } \
        } \
    }
#define CMDMACRO_PC_BuildShootCommandFromInput(name, NAME) \
    c->output.shoot.cmd.firecmd = false; \
    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) { \
        CMD_ModuleMask_t moduleMask = behaviorModuleTable[i]; \
        if (CMD_PC_IsMaskMatch(c, moduleMask)) { \
            if (CMD_PC_IsBehaviorTriggered(c, i)) { \
                behaviorHandlerFuncTable[i].func(c); \
            } \
        } \
    } \
    memcpy(c->input.pc.lastData->name.keyboard.key, c->input.pc.data->name.keyboard.key, sizeof(c->input.pc.data->name.keyboard.key)); \
    c->input.pc.lastData->name.mouse.l_click = c->input.pc.data->name.mouse.l_click; \
    c->input.pc.lastData->name.mouse.r_click = c->input.pc.data->name.mouse.r_click;
/* Private typedef ---------------------------------------------------------- */
typedef int8_t (*CMD_BehaviorFunc)(CMD_t *c);
typedef struct {
    CMD_Behavior_t behavior;
    CMD_BehaviorFunc func;
} CMD_BehaviorHandlerFunc_t;

union CMD_PC_Data_t {
    CMD_RCType_TABLE(CMDMACRO_VAR_PCDATA)
 };
union CMD_PC_LastData_t {
    CMD_RCType_TABLE(CMDMACRO_VAR_PCDATA)
 };
/* Private variables -------------------------------------------------------- */
/* 行为处理函数指针数组 */
CMD_BehaviorHandlerFunc_t behaviorHandlerFuncTable[CMD_BEHAVIOR_NUM] = {
    CMD_PCBehavior_TABLE(CMDMACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY)
};
/* 行为模块映射表 */
static const CMD_ModuleMask_t behaviorModuleTable[CMD_BEHAVIOR_NUM] = {
    CMD_PCBehavior_TABLE(CMDMACRO_FOR_BUILD_MODULE_TABLE)
};

/* 静态缓冲区 */
CMD_PC_Data_t pc_buffer;
CMD_PC_LastData_t pc_lastdata_buffer;
/* Private function  -------------------------------------------------------- */
int8_t Cmd_PC_Get(CMD_Input_PC_t *pc){
    CMD_RCType_TABLE(CMDMACRO_Cmd_PC_Get);
    return CMD_OK;
}

static int8_t Cmd_PC_HandleBehaviorFORE(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vy += c->params->pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorBACK(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vy -= c->params->pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorLEFT(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vx -= c->params->pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorRIGHT(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vx += c->params->pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorACCELERATE(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vx *= c->params->pc.sensitivity.move_fast_sense;
    c->output.chassis.cmd.ctrl_vec.vy *= c->params->pc.sensitivity.move_fast_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorDECELEBRATE(CMD_t *c){
    c->output.chassis.cmd.ctrl_vec.vx *= c->params->pc.sensitivity.move_slow_sense;
    c->output.chassis.cmd.ctrl_vec.vy *= c->params->pc.sensitivity.move_slow_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorFIRE(CMD_t *c){
    c->output.shoot.cmd.firecmd = true;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorFIRE_MODE(CMD_t *c){
    c->output.shoot.cmd.mode++;
    c->output.shoot.cmd.mode %= SHOOT_MODE_NUM;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorBUFF(CMD_t *c){
//    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
//        cmd->host_overwrite = false;
//        cmd->ai_status = AI_STATUS_STOP;
//    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
//        // 自瞄模式中切换失败提醒
//    } else {
//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
//        cmd->ai_status = AI_STATUS_HITSWITCH;
//        cmd->host_overwrite = true;
//    }
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorAUTOAIM(CMD_t *c){
//    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
//        cmd->host_overwrite = false;
//        cmd->ai_status = AI_STATUS_STOP;
//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
//    } else {
//        cmd->ai_status = AI_STATUS_AUTOAIM;
//        cmd->host_overwrite = true;
//        CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
//    }
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorOPENCOVER(CMD_t *c){
//    c->shoot.cover_open = !c->shoot.cover_open;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorROTOR(CMD_t *c){
    c->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
    c->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorREVTRIG(CMD_t *c){
//    c->output.shoot.cmd.reverse_trig = true;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorFOLLOWGIMBAL35(CMD_t *c){
    c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
    return CMD_OK;
}

static inline CMD_PCValue_t CMD_PC_BehaviorToValue(CMD_t *c,
                                               CMD_Behavior_t behavior) {
  return c->params->pc.map.key_map[behavior].key;
}

static inline CMD_TriggerType_t CMD_PC_BehaviorToActive(CMD_t *c,
                                                    CMD_Behavior_t behavior) {
  return c->params->pc.map.key_map[behavior].trigger_type;
}

static inline bool CMD_PC_IsMaskMatch(CMD_t *c, CMD_ModuleMask_t module_mask) {
    /* 构建当前各模块输入源状态的掩码 */
    CMD_ModuleMask_t current_pc_mask = 0;
    if (c->output.chassis.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_CHASSIS;
    if (c->output.gimbal.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_GIMBAL;
    if (c->output.shoot.source == CMD_SRC_PC) current_pc_mask |= CMD_MODULE_SHOOT;
    
    /* 检测并集 */
    return (module_mask & current_pc_mask) == module_mask;
    /* 检测交集 */
//    return (module_mask & current_pc_mask) != 0;
}

static inline bool CMD_PC_IsBehaviorTriggered(CMD_t *c, CMD_Behavior_t behavior) {
  CMD_PCValue_t value = CMD_PC_BehaviorToValue(c, behavior);
  CMD_TriggerType_t active = CMD_PC_BehaviorToActive(c, behavior);

  bool now_key_pressed, last_key_pressed;

  /* 按下按键为鼠标左、右键 */
  if (value == CMD_L_CLICK) {
    now_key_pressed = c->input.pc.data->dr16.mouse.l_click;
    last_key_pressed = c->input.pc.lastData->dr16.mouse.l_click;
  } else if (value == CMD_R_CLICK) {
    now_key_pressed = c->input.pc.data->dr16.mouse.r_click;
    last_key_pressed = c->input.pc.lastData->dr16.mouse.r_click;
  } else {
    /* 直接用布尔值 */
    now_key_pressed = c->input.pc.data->dr16.keyboard.key[value];
    last_key_pressed = c->input.pc.lastData->dr16.keyboard.key[value];
    /* 或者用位掩码 */
    // now_key_pressed = c->input.pc.data.keyboard.bitmask & (1u << value);
    // last_key_pressed = c->input.pc.lastData.keyboard.bitmask & (1u << value);
  }

  switch (active) {
    case CMD_ACTIVE_RISING_EDGE:
      return !now_key_pressed && last_key_pressed;
    case CMD_ACTIVE_FALLING_EDGE:
      return now_key_pressed && !last_key_pressed;
    case CMD_ACTIVE_PRESSED:
      return now_key_pressed;
  }
    return false;
}

static int8_t Cmd_PC_BuildChassisCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    // c->output.chassis.cmd.ctrl_vec.vx = c->output.chassis.cmd.ctrl_vec.vy = 0.0f;
    // for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
    //     CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
    //     if (CMD_PC_IsMaskMatch(c, moduleMask)) {
    //         if (CMD_PC_IsBehaviorTriggered(c, i)) {
    //             behaviorHandlerFuncTable[i].func(c);
    //         }
    //     }
    // }
    CMD_RCType_TABLE(CMDMACRO_PC_BuildChassisCommandFromInput);
    return CMD_OK;
}

static int8_t Cmd_PC_BuildGimbalCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    // c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
    // c->output.gimbal.cmd.delta_yaw = (float)c->input.pc.data->dr16.mouse.x * c->timer.dt * c->params->pc.sensitivity.sens_mouse;
    // c->output.gimbal.cmd.delta_pit = (float)c->input.pc.data->dr16.mouse.y * c->timer.dt * c->params->pc.sensitivity.sens_mouse;
    // for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
    //     CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
    //     if (CMD_PC_IsMaskMatch(c, moduleMask)) {
    //         if (CMD_PC_IsBehaviorTriggered(c, i)) {
    //             behaviorHandlerFuncTable[i].func(c);
    //         }
    //     }
    // }
    CMD_RCType_TABLE(CMDMACRO_PC_BuildGimbalCommandFromInput);
    return CMD_OK;
}

static int8_t Cmd_PC_BuildShootCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    // c->output.shoot.cmd.firecmd = false;
    // for (size_t i = 0; i < CMD_RCType_TABLE(CMDMACRO_KEYNUM); i++) {
    //     CMD_ModuleMask_t moduleMask = behaviorModuleTable[i];
    //     if (CMD_PC_IsMaskMatch(c, moduleMask)) {
    //         if (CMD_PC_IsBehaviorTriggered(c, i)) {
    //             behaviorHandlerFuncTable[i].func(c);
    //         }
    //     }
    // }
    // memcpy(c->input.pc.lastData->dr16.keyboard.key, c->input.pc.data->dr16.keyboard.key, sizeof(c->input.pc.data->dr16.keyboard.key));
    // c->input.pc.lastData->dr16.mouse.l_click = c->input.pc.data->dr16.mouse.l_click;
    // c->input.pc.lastData->dr16.mouse.r_click = c->input.pc.data->dr16.mouse.r_click;
    CMD_RCType_TABLE(CMDMACRO_PC_BuildShootCommandFromInput);
    return CMD_OK;
}
#else
static int8_t Cmd_PC_Get(CMD_Input_PC_t *pc) {
    pc->online=false;
    return CMD_OK;
}
static int8_t Cmd_PC_BuildChassisCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
static int8_t Cmd_PC_BuildGimbalCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
static int8_t Cmd_PC_BuildShootCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
#endif

/*************************************************************************************************************************************/
/****************************************************************NUC******************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
int8_t Cmd_NUC_Get(CMD_Input_NUC_t *nuc) {
    nuc->online=0;
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */

int8_t Cmd_NUC_BuildChassisCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}

int8_t Cmd_NUC_BuildGimbalCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
int8_t Cmd_NUC_BuildShootCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}

/*************************************************************************************************************************************/
/***************************************************************REF*******************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
int8_t Cmd_REF_Get(CMD_Input_REF_t *nuc) {
    nuc->online=0;
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */

int8_t Cmd_REF_BuildChassisCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}

int8_t Cmd_REF_BuildGimbalCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
int8_t Cmd_REF_BuildShootCommandFromInput(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }

    return CMD_OK;
}
/*************************************************************************************************************************************/
/***************************************************************仲裁器****************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
static inline bool Cmd_isREFOnline(CMD_t *c){return c->input.ref.online;}
static inline bool Cmd_isNUCOnline(CMD_t *c){return c->input.nuc.online;}
static inline bool Cmd_isRCOnline(CMD_t *c){return c->input.rc.online;}
static inline bool Cmd_isPCOnline(CMD_t *c){return c->input.pc.online;}
CMD_InputSource_t Cmd_GetHighestPrioritySource(CMD_t *c) {
    for (int i = 0; i < CMD_SRC_NUM; i++) {
        CMD_InputSource_t source = c->params->sourcePriorityConfigs[i].source;
        switch (source) {
            case CMD_SRC_REF:
                if (Cmd_isREFOnline(c)) {
                    return CMD_SRC_REF;
                }
                break;
            case CMD_SRC_NUC:
                if (Cmd_isNUCOnline(c)) {
                    return CMD_SRC_NUC;
                }
                break;
            case CMD_SRC_RC:
                if (Cmd_isRCOnline(c)) {
                    return CMD_SRC_RC;
                }
                break;
            case CMD_SRC_PC:
                if (Cmd_isPCOnline(c)) {
                    return CMD_SRC_PC;
                }
        }
    }
    return CMD_SRC_ERR;
}

int8_t Cmd_Arbiter(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    CMD_InputSource_t source = Cmd_GetHighestPrioritySource(c);

    c->output.chassis.source = source;
    c->output.gimbal.source = source;
    c->output.shoot.source = source;



    return CMD_OK;
}

int8_t Cmd_Switch_RCorPC(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    
    return CMD_OK;
}

/*************************************************************************************************************************************/
/***************************************************************主结构*****************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
typedef int8_t (*CMD_BuildCommandFunc)(CMD_t *c);
typedef struct {
    CMD_InputSource_t source;
    CMD_BuildCommandFunc chassisFunc;
    CMD_BuildCommandFunc gimbalFunc;
    CMD_BuildCommandFunc shootFunc;
} CMD_SourceHandler_t;
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
    {CMD_SRC_RC, Cmd_RC_BuildChassisCommandFromInput, Cmd_RC_BuildGimbalCommandFromInput, Cmd_RC_BuildShootCommandFromInput},
    {CMD_SRC_PC, Cmd_PC_BuildChassisCommandFromInput, Cmd_PC_BuildGimbalCommandFromInput, Cmd_PC_BuildShootCommandFromInput},
    {CMD_SRC_NUC, Cmd_NUC_BuildChassisCommandFromInput, Cmd_NUC_BuildGimbalCommandFromInput, Cmd_NUC_BuildShootCommandFromInput},
    {CMD_SRC_REF, Cmd_REF_BuildChassisCommandFromInput, Cmd_REF_BuildGimbalCommandFromInput, Cmd_REF_BuildShootCommandFromInput},
};
/* Private function  -------------------------------------------------------- */
int8_t Cmd_OFFLINE(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    c->output.chassis.cmd.mode =CHASSIS_MODE_RELAX;
    c->output.gimbal.cmd.mode =GIMBAL_MODE_RELAX;
    c->output.shoot.cmd.mode =SHOOT_MODE_SAFE;
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */
int8_t Cmd_Init(CMD_t *c, CMD_Params_t *params) {

    if (c == NULL || params == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    c->params = params;
    c->input.rc.data=&rc_buffer; 
    #ifndef CMD_NOPC_FLAG
    c->input.pc.lastData=&pc_lastdata_buffer;
    c->input.pc.data=&pc_buffer;
    #endif
    return CMD_OK;
}

int8_t Cmd_GenerateCommand(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
    Cmd_Arbiter(c);
    Cmd_RC_Get(&c->input.rc);
    Cmd_PC_Get(&c->input.pc);
    Cmd_NUC_Get(&c->input.nuc);
    Cmd_REF_Get(&c->input.ref);
    c->timer.now 		  	=BSP_TIME_Get_us() / 1000000.0f;
    c->timer.dt 			=(BSP_TIME_Get_us() - c->timer.last) / 1000000.0f;
    c->timer.last	        =BSP_TIME_Get_us();
    if (c->output.chassis.source >= CMD_SRC_NUM || c->output.gimbal.source >= CMD_SRC_NUM || c->output.shoot.source >= CMD_SRC_NUM) {
        Cmd_OFFLINE(c);
        return CMD_ERR_SOURCE; // 输入源错误
    }
    sourceHandlers[c->output.chassis.source].chassisFunc(c);
    sourceHandlers[c->output.gimbal.source].gimbalFunc(c);
    sourceHandlers[c->output.shoot.source].shootFunc(c);
    return CMD_OK;
}