/*
  控制命令
*/
#include "module/cmd.h"
#include <stdint.h>
#include <string.h>

/*************************************************************************************************************************************/
/***************************************************************仲裁器****************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */

bool isREFOnline(CMD_t *c){return c->inputData.ref.online;}
bool isNUCOnline(CMD_t *c){return c->inputData.nuc.online;}
bool isRCOnline(CMD_t *c){return c->inputData.rc.online;}
bool isPCOnline(CMD_t *c){return c->inputData.pc.online;}
void PriorityConfigsRanking(CMD_Param_t *param) {
    static bool init=false;
    if(init) return;
    for (int i = 0; i < CMD_SRC_NUM - 1; i++) {
        for (int j = 0; j < CMD_SRC_NUM - i - 1; j++) {
            if (param->sourcePriorityConfigs[j].priority < param->sourcePriorityConfigs[j + 1].priority) {
                CMD_SourcePriorityConfig_t temp = param->sourcePriorityConfigs[j];
                param->sourcePriorityConfigs[j] = param->sourcePriorityConfigs[j + 1];
                param->sourcePriorityConfigs[j + 1] = temp;
            }
        }
    }
    init=!init;
}
CMD_InputSource_t 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_REFEREE:
                if (isREFOnline(c)) {
                    return CMD_SRC_REFEREE;
                }
                break;
            case CMD_SRC_NUC:
                if (isNUCOnline(c)) {
                    return CMD_SRC_NUC;
                }
                break;
            case CMD_SRC_RC:
                if (isRCOnline(c)) {
                    return CMD_SRC_RC;
                }
                break;
            case CMD_SRC_PC:
                if (isPCOnline(c)) {
                    return CMD_SRC_PC;
                }
        }
    }
    return CMD_SRC_ERR;
}

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

    PriorityConfigsRanking(&c->params);

    CMD_InputSource_t source = getHighestPrioritySource(c);

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

    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */



/*************************************************************************************************************************************/
/****************************************************************RC*******************************************************************/
/*************************************************************************************************************************************/
/* API ---------------------------------------------------------------------- */
/* Mrobot生成 */
#define RC_SELECT_Index 1
/* 扩展接口 */
#if RC_SELECT_Index == 0
#define FOR_EACH_RC(X) X(dr16, DR16)
#elif RC_SELECT_Index == 1
#define FOR_EACH_RC(X) X(at9s, AT9S)
#endif
/* Includes ----------------------------------------------------------------- */
#if RC_SELECT_Index == 0
#include "device/dr16.h"
#elif RC_SELECT_Index == 1
#include "device/at9s_pro.h"
#endif
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
#define RC_X_FIELD_MACRO(name, NAME) DEVICE_##NAME##_t name;
#define RC_X_EXTERN_MACRO(name, NAME) extern DEVICE_##NAME##_t name##_out;
#define RC_X_COPYDEFINE_MACRO(name, NAME) \
    static void copy_##name(rc_inputdata_u *dst) { dst->name = name##_out; }
#define RC_X_COPYREFERENCE_MACRO(name, NAME) copy_##name,
union rc_inputdata_u{
    FOR_EACH_RC(RC_X_FIELD_MACRO)
};
FOR_EACH_RC(RC_X_EXTERN_MACRO)
FOR_EACH_RC(RC_X_COPYDEFINE_MACRO)
/* Private variables -------------------------------------------------------- */
/*静态缓冲区*/
static rc_inputdata_u rc_buffer;
/* Private function  -------------------------------------------------------- */
int8_t Cmd_Get_rc(CMD_InputData_RC_t *rc)
{
    FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer); 
    rc->inputData = &rc_buffer;                     
    rc->type = RC_SELECT_Index;
    return CMD_OK;
}


int8_t Cmd_BuildChassisCommandFromInput_rc(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if RC_SELECT_Index == 0
    c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
    switch (c->inputData.rc.inputData->at9s.data.key_E) {
    case AT9S_CMD_SW_DOWN:
      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    case AT9S_CMD_SW_MID:
      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
      break;
    case AT9S_CMD_SW_UP:
      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
      break;
    default:
      c->outCmd.chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    }
    c->outCmd.chassis.cmd.ctrl_vec.vx = c->inputData.rc.inputData->at9s.data.ch_l_y;
    c->outCmd.chassis.cmd.ctrl_vec.vy = c->inputData.rc.inputData->at9s.data.ch_l_x;
    c->outCmd.chassis.cmd.ctrl_vec.wz = c->inputData.rc.inputData->at9s.data.ch_r_x;
#endif
    return CMD_OK;
}

int8_t Cmd_BuildGimbalCommandFromInput_rc(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if RC_SELECT_Index == 0
    c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
	switch (c->inputData.rc.inputData->at9s.data.key_G) {
    case AT9S_CMD_SW_DOWN:
      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
      c->outCmd.gimbal.cmd.delta_pit = 0.0f;
      break;
    case AT9S_CMD_SW_MID:
      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
      c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
      break;
    case AT9S_CMD_SW_UP:
      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->outCmd.gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
      c->outCmd.gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
      break;
    default:
      c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->outCmd.gimbal.cmd.delta_yaw = 0.0f;
      c->outCmd.gimbal.cmd.delta_pit = 0.0f;
      break;
    }
#endif
    return CMD_OK;
}


int8_t Cmd_BuildShootCommandFromInput_rc(CMD_t *c) {
    if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
    }
#if RC_SELECT_Index == 0
    c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
    switch (c->inputData.rc.inputData->at9s.data.key_C) {
    case AT9S_CMD_SW_DOWN:
	  c->outCmd.shoot.cmd.online = at9s_out.online;
      c->outCmd.shoot.cmd.ready    = true;
      c->outCmd.shoot.cmd.firecmd  = true;
      break;
    case AT9S_CMD_SW_MID:
	  c->outCmd.shoot.cmd.online = at9s_out.online;
      c->outCmd.shoot.cmd.ready    = true;
      c->outCmd.shoot.cmd.firecmd  = false;
      break;
    case AT9S_CMD_SW_UP:
	  c->outCmd.shoot.cmd.online = at9s_out.online;
      c->outCmd.shoot.cmd.ready    = false;
      c->outCmd.shoot.cmd.firecmd  = false;
      break;
    default:
	  c->outCmd.shoot.cmd.online = at9s_out.online;
      c->outCmd.shoot.cmd.ready    = false;
      c->outCmd.shoot.cmd.firecmd  = false;
      break;
    }
#endif
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */


/*************************************************************************************************************************************/
/*****************************************************************PC******************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
typedef int8_t (*CMD_BehaviorFunc)(CMD_t *c);
typedef struct {
    CMD_Behavior_t behavior;
    CMD_BehaviorFunc func;
} CMD_BehaviorHandlerFunc_t;

/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
#define PC_MACRO_BEHAVIOR_TABLE(X) \
    X(FORE)                 \
    X(BACK)                 \
    X(LEFT)                 \
    X(RIGHT)                \
    X(ACCELERATE)           \
    X(DECELEBRATE)          \
    X(FIRE)                 \
    X(FIRE_MODE)            \
    X(BUFF)                 \
    X(AUTOAIM)              \
    X(OPENCOVER)            \
    X(ROTOR)                \
    X(REVTRIG)               \
    X(FOLLOWGIMBAL35)       
#define PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION(BEHAVIOR) \
    static int8_t Cmd_PC_HandleBehavior##BEHAVIOR(CMD_t *c);

#define PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY(BEHAVIOR) \
    {CMD_BEHAVIOR_##BEHAVIOR, Cmd_PC_HandleBehavior##BEHAVIOR},

PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_DECLARE_HANDLER_FUNCTION)
/* Private variables -------------------------------------------------------- */
CMD_BehaviorHandlerFunc_t behaviorHandlerFunc[] = {
    PC_MACRO_BEHAVIOR_TABLE(PC_MACRO_FOR_BUILD_BEHAVIOR_HANDLER_ARRAY)
};
/* Private function  -------------------------------------------------------- */
static int8_t Cmd_PC_HandleBehaviorFORE(CMD_t *c){
    c->outCmd.chassis.cmd.ctrl_vec.vy += c->params.pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorBACK(CMD_t *c){
    c->outCmd.chassis.cmd.ctrl_vec.vy -= c->params.pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorLEFT(CMD_t *c){
    c->outCmd.chassis.cmd.ctrl_vec.vx -= c->params.pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorRIGHT(CMD_t *c){
    c->outCmd.chassis.cmd.ctrl_vec.vx += c->params.pc.sensitivity.move_sense;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorACCELERATE(CMD_t *c){
    c->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_fast_sense;
    c->outCmd.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->outCmd.chassis.cmd.ctrl_vec.vx *= c->params.pc.sensitivity.move_slow_sense;
    c->outCmd.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->outCmd.shoot.cmd.firecmd = true;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorFIRE_MODE(CMD_t *c){
    c->outCmd.shoot.cmd.mode++;
    c->outCmd.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->outCmd.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
    c->outCmd.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorREVTRIG(CMD_t *c){
//    c->outCmd.shoot.cmd.reverse_trig = true;
    return CMD_OK;
}
static int8_t Cmd_PC_HandleBehaviorFOLLOWGIMBAL35(CMD_t *c){
    c->outCmd.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35;
    return CMD_OK;
}

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

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

int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
{
    pc->online = false;
//    pc->data=
    return CMD_OK;
}

static bool CMD_BehaviorOccurredRc(CMD_t *c, CMD_Behavior_t behavior) {
  CMD_KeyValue_t key = CMD_BehaviorToKey(c, behavior);
  CMD_TriggerType_t active = CMD_BehaviorToActive(c, behavior);

  bool now_key_pressed, last_key_pressed;

  /* 按下按键为鼠标左、右键 */
  if (key == CMD_L_CLICK) {
    now_key_pressed = c->inputData.pc.data.mouse.l_click;
    last_key_pressed = c->inputData.pc.lastData.mouse.l_click;
  } else if (key == CMD_R_CLICK) {
    now_key_pressed = c->inputData.pc.data.mouse.r_click;
    last_key_pressed = c->inputData.pc.lastData.mouse.r_click;
  } else {
    now_key_pressed = c->inputData.pc.data.key & (1u << key);
    last_key_pressed = c->inputData.pc.lastData.key & (1u << key);
  }

  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;
  }
}

/* Exported functions ------------------------------------------------------- */



static void CMD_PcLogic(CMD_t *c, float dt_sec) {
    c->outCmd.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;

    c->outCmd.gimbal.cmd.delta_yaw = (float)c->inputData.pc.data.mouse.x * dt_sec * c->params.pc.sensitivity.sens_mouse;
    c->outCmd.gimbal.cmd.delta_pit = (float)c->inputData.pc.data.mouse.y * dt_sec * c->params.pc.sensitivity.sens_mouse;
    c->outCmd.chassis.cmd.ctrl_vec.vx = c->outCmd.chassis.cmd.ctrl_vec.vy = 0.0f;

    c->outCmd.shoot.cmd.firecmd = false;

    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
        if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
            behaviorHandlerFunc[i].func(c);
        }
    }

    c->inputData.pc.lastData.key = c->inputData.pc.data.key;
    c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
    c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;
}

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

    return CMD_OK;
}

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

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

     c->outCmd.shoot.cmd.firecmd = false;

    for (size_t i = 0; i < CMD_BEHAVIOR_NUM; i++) {
        if (CMD_BehaviorOccurredRc(&c, behaviorHandlerFunc[i].behavior)) {
            behaviorHandlerFunc[i].func(c);
        }
    }

    c->inputData.pc.lastData.key = c->inputData.pc.data.key;
    c->inputData.pc.lastData.mouse.l_click = c->inputData.pc.data.mouse.l_click;
    c->inputData.pc.lastData.mouse.r_click = c->inputData.pc.data.mouse.r_click;

    return CMD_OK;
}
/*************************************************************************************************************************************/
/****************************************************************NUC******************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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

    return CMD_OK;
}

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

    return CMD_OK;
}
int8_t Cmd_BuildShootCommandFromInput_nuc(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  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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

    return CMD_OK;
}

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

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

    return CMD_OK;
}

/*************************************************************************************************************************************/
/***************************************************************分发命令***************************************************************/
/*************************************************************************************************************************************/

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

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;

CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
    {CMD_SRC_RC, Cmd_BuildChassisCommandFromInput_rc, Cmd_BuildGimbalCommandFromInput_rc, Cmd_BuildShootCommandFromInput_rc},
    {CMD_SRC_PC, Cmd_BuildChassisCommandFromInput_pc, Cmd_BuildGimbalCommandFromInput_pc, Cmd_BuildShootCommandFromInput_pc},
    {CMD_SRC_NUC, Cmd_BuildChassisCommandFromInput_nuc, Cmd_BuildGimbalCommandFromInput_nuc, Cmd_BuildShootCommandFromInput_nuc},
    {CMD_SRC_REFEREE, Cmd_BuildChassisCommandFromInput_referee, Cmd_BuildGimbalCommandFromInput_referee, Cmd_BuildShootCommandFromInput_referee},
};

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

    Cmd_Arbiter(c);
    switch (c->outCmd.chassis.source) {
        case CMD_SRC_RC:
            sourceHandlers[CMD_SRC_RC].chassisFunc(c);
            break;
        case CMD_SRC_PC:
            sourceHandlers[CMD_SRC_PC].chassisFunc(c);
            break;
        case CMD_SRC_NUC:
            sourceHandlers[CMD_SRC_NUC].chassisFunc(c);
            break;
        case CMD_SRC_REFEREE:
            sourceHandlers[CMD_SRC_REFEREE].chassisFunc(c);
            break;
        default:
            break;
    }
    switch (c->outCmd.gimbal.source) {
        case CMD_SRC_RC:
            sourceHandlers[CMD_SRC_RC].gimbalFunc(c);
            break;
        case CMD_SRC_PC:
            sourceHandlers[CMD_SRC_PC].gimbalFunc(c);
            break;
        case CMD_SRC_NUC:
            sourceHandlers[CMD_SRC_NUC].gimbalFunc(c);
            break;
        case CMD_SRC_REFEREE:
            sourceHandlers[CMD_SRC_REFEREE].gimbalFunc(c);
            break;
        default:
            break;
    }
    switch (c->outCmd.shoot.source) {
        case CMD_SRC_RC:
            sourceHandlers[CMD_SRC_RC].shootFunc(c);
            break;
        case CMD_SRC_PC:
            sourceHandlers[CMD_SRC_PC].shootFunc(c);
            break;
        case CMD_SRC_NUC:
            sourceHandlers[CMD_SRC_NUC].shootFunc(c);
            break;
        case CMD_SRC_REFEREE:
            sourceHandlers[CMD_SRC_REFEREE].shootFunc(c);
            break;
        default:
            break;
    }

    return CMD_OK;
}