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

#include <stdint.h>
#include <string.h>
#include "bsp/time.h"
#include "module/gimbal.h"

/*************************************************************************************************************************************/
/****************************************************************RC*******************************************************************/
/*************************************************************************************************************************************/
/* Private macro ------------------------------------------------------------ */
/* 声明外部rc数据变量 */
#define CMDMACRO_EXTERNAL_RCDATA(name, NAME) extern NAME##_t name;

#define CMDMACRO_NAME_EXPANSION_1(name, NAME) name
/* 声明CMD结构体的RC数据变量 */
#define CMDMACRO_VAR_RCDATA(name, NAME) NAME##_DataRC_t name;
/* 宏生成Cmd_RC_Get函数内容 */
#define CMDMACRO_Cmd_RC_Get(name, NAME) \
    c->input.rc.name= name.data.rc; \
    c->input.rc.online=name.header.online; \
    c->input.rc.type=CMD_RCTypeTable_Index;

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

/* Private variables -------------------------------------------------------- */
/*静态缓冲区*/

/* Private function  -------------------------------------------------------- */
int8_t Cmd_RC_Get(CMD_t *c, CMD_RCType_TABLE(CMDMACRO_NAME_EXPANSION)){
    // bool online= dr16->header.online;
    // rc_buffer.dr16=dr16->data.rc;
    // c->input.rc.online=online;
    // c->input.rc.type=CMD_RCTypeTable_Index;
    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.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.dr16.ch_r_x;
    c->output.chassis.cmd.ctrl_vec.vy = c->input.rc.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.dr16.sw_l) {
    case DR16_SW_DOWN:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.dr16.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.dr16.ch_l_y * 1.5f;
      break;
    case DR16_SW_MID:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.dr16.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.dr16.ch_l_y * 1.5f;
      break;
    case DR16_SW_UP:
      c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
      c->output.gimbal.cmd.delta_yaw = -c->input.rc.dr16.ch_l_x * 2.0f;
      c->output.gimbal.cmd.delta_pit = -c->input.rc.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
    if (c->input.rc.online) {
        c->output.shoot.cmd.mode=SHOOT_MODE_SINGLE;
    } else {
        c->output.shoot.cmd.mode=SHOOT_MODE_SAFE;
    }
    switch (c->input.rc.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)       \
  X(GIMBAL_MODE, CMD_MODULE_GIMBAL)

/* 行为处理函数声明宏 */
#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_Cmd_PC_Get(name, NAME) \
  c->input.pc.online=name.header.online;\
  c->input.pc.name= name.data.pc;
#define CMDMACRO_PC_IsBehaviorTriggered(name, NAME) \
  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.name.mouse.l_click; \
    last_key_pressed = c->input.pc.last##name.mouse.l_click; \
  } else if (value == CMD_R_CLICK) { \
    now_key_pressed = c->input.pc.name.mouse.r_click; \
    last_key_pressed = c->input.pc.last##name.mouse.r_click; \
  } else { \
    now_key_pressed = c->input.pc.name.keyboard.key[value]; \
    last_key_pressed = c->input.pc.last##name.keyboard.key[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;
#define CMDMACRO_PC_BuildChassisCommandFromInput(name, NAME) \
  if (c->input.pc.online) {c->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;} \
  else {c->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;} \
  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); \
          } \
      } \
  }\
  return CMD_OK;
#define CMDMACRO_PC_BuildGimbalCommandFromInput(name, NAME) \
  static bool init = false; \
  if (!init) {c->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;init=!init;}\
  if(!c->input.pc.online) {c->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;} \
  c->output.gimbal.cmd.delta_yaw = (float)-c->input.pc.name.mouse.x * c->timer.dt * c->params->pc.sensitivity.sens_mouse; \
  c->output.gimbal.cmd.delta_pit = (float)1.5f*c->input.pc.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); \
          } \
      } \
  }\
  return CMD_OK;
#define CMDMACRO_PC_BuildShootCommandFromInput(name, NAME) \
  if (!c->input.pc.online) {c->output.shoot.cmd.mode = SHOOT_MODE_SAFE;} \
  c->output.shoot.cmd.ready = true; \
  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); \
          } \
      } \
  } \
  return CMD_OK;\
  memcpy(c->input.pc.last##name.keyboard.key, c->input.pc.name.keyboard.key, sizeof(c->input.pc.name.keyboard.key)); \
  c->input.pc.last##name.mouse.l_click = c->input.pc.name.mouse.l_click; \
  c->input.pc.last##name.mouse.r_click = c->input.pc.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;

/* 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)
};
/* Private function  -------------------------------------------------------- */
int8_t Cmd_PC_Get(CMD_t *c, CMD_RCType_TABLE(CMDMACRO_NAME_EXPANSION)){
  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 int8_t Cmd_PC_HandleBehaviorGIMBAL_MODE(CMD_t *c){
  
  c->output.gimbal.cmd.mode++;
  c->output.gimbal.cmd.mode %= GIMBAL_MODE_NUM;
  while (c->output.gimbal.cmd.mode<=0) {c->output.gimbal.cmd.mode++;}
  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) {
  if (c == NULL) {
        return CMD_ERR_NULL; // 参数错误
  }
  CMD_RCType_TABLE(CMDMACRO_PC_IsBehaviorTriggered);
}

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

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

static int8_t Cmd_PC_BuildShootCommandFromInput(CMD_t *c) {
  if (c == NULL) {
    return CMD_ERR_NULL; // 参数错误
  }
  CMD_RCType_TABLE(CMDMACRO_PC_BuildShootCommandFromInput);
}
#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];
    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;
        }
        break;
      case CMD_SRC_NUM:
        return CMD_ERR_SOURCE;
    }
  }
  return CMD_ERR_SOURCE;
}

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

int8_t Cmd_Get(CMD_t *c, 
              CMD_RCType_TABLE(CMDMACRO_NAME_EXPANSION)){
              // CMD_Input_NUC_t *nuc, 
              // CMD_Input_REF_t *ref) {
  if (c == NULL) {
      return CMD_ERR_NULL; // 参数错误
  }
  Cmd_RC_Get(c,CMD_RCType_TABLE(CMDMACRO_NAME_EXPANSION_1));
  Cmd_PC_Get(c,CMD_RCType_TABLE(CMDMACRO_NAME_EXPANSION_1));
  // Cmd_NUC_Get(&c->input.nuc);
  // Cmd_REF_Get(&c->input.ref);
  
  return CMD_OK;
}

int8_t Cmd_GenerateCommand(CMD_t *c) {
  if (c == NULL) {
      return CMD_ERR_NULL; // 参数错误
  }
  Cmd_Arbiter(c);
  
  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;
}