/*
  控制命令
*/
#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->input_ref.online;}
bool isNUCOnline(CMD_t *c){return c->input_nuc.online;}
bool isRCOnline(CMD_t *c){return c->input_rc.online;}
bool isPCOnline(CMD_t *c){return c->input_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->rcSourcePriorityConfigs[j].priority < param->rcSourcePriorityConfigs[j + 1].priority) {
                CMD_RCSourcePriorityConfig_t temp = param->rcSourcePriorityConfigs[j];
                param->rcSourcePriorityConfigs[j] = param->rcSourcePriorityConfigs[j + 1];
                param->rcSourcePriorityConfigs[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.rcSourcePriorityConfigs[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_PC; /*默认使用键盘鼠标*/
}

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

    PriorityConfigsRanking(&c->params);

    CMD_InputSource_t source = getHighestPrioritySource(c);

    c->out_chassis.source = source;
    c->out_gimbal.source = source;
    c->out_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(_) _(dr16, DR16)
#elif RC_SELECT_Index == 1
#define FOR_EACH_RC(_) _(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->input_rc.inputData->at9s.data.key_E) {
    case AT9S_CMD_SW_DOWN:
      c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    case AT9S_CMD_SW_MID:
      c->out_chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
      break;
    case AT9S_CMD_SW_UP:
      c->out_chassis.cmd.mode = CHASSIS_MODE_ROTOR;
      break;
    default:
      c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
      break;
    }
    c->out_chassis.cmd.ctrl_vec.vx = c->input_rc.inputData->at9s.data.ch_l_y;
    c->out_chassis.cmd.ctrl_vec.vy = c->input_rc.inputData->at9s.data.ch_l_x;
    c->out_chassis.cmd.ctrl_vec.wz = c->input_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->input_rc.inputData->at9s.data.key_G) {
    case AT9S_CMD_SW_DOWN:
      c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->out_gimbal.cmd.delta_yaw = 0.0f;
      c->out_gimbal.cmd.delta_pit = 0.0f;
      break;
    case AT9S_CMD_SW_MID:
      c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
      c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
      break;
    case AT9S_CMD_SW_UP:
      c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
      c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
      break;
    default:
      c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
      c->out_gimbal.cmd.delta_yaw = 0.0f;
      c->out_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->input_rc.inputData->at9s.data.key_C) {
    case AT9S_CMD_SW_DOWN:
	  c->out_shoot.cmd.online = at9s_out.online;
      c->out_shoot.cmd.ready    = true;
      c->out_shoot.cmd.firecmd  = true;
      break;
    case AT9S_CMD_SW_MID:
	  c->out_shoot.cmd.online = at9s_out.online;
      c->out_shoot.cmd.ready    = true;
      c->out_shoot.cmd.firecmd  = false;
      break;
    case AT9S_CMD_SW_UP:
	  c->out_shoot.cmd.online = at9s_out.online;
      c->out_shoot.cmd.ready    = false;
      c->out_shoot.cmd.firecmd  = false;
      break;
    default:
	  c->out_shoot.cmd.online = at9s_out.online;
      c->out_shoot.cmd.ready    = false;
      c->out_shoot.cmd.firecmd  = false;
      break;
    }
#endif
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */


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

/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
{
    FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer); 
    rc->inputData = &rc_buffer;                     
    rc->type = RC_SELECT_Index;
    return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */


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; // 参数错误
    }

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