/*
  控制命令
*/

#include "cmd.h"

#include <string.h>



/**
 * @brief 解析rc行为逻辑
 *
 * @param rc 遥控器数据
 * @param cmd 主结构体
 * @param dt_sec 两次解析的间隔
 */
static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  switch (rc->sw_l) {
      /* 左拨杆相应行为选择和解析 */
    case CMD_SW_UP:
      cmd->chassis.mode = CHASSIS_MODE_RELAX;
      break;

    case CMD_SW_MID:
      cmd->chassis.mode = CHASSIS_MODE_DAMP;
      break;

    case CMD_SW_DOWN:
      cmd->chassis.mode = CHASSIS_MODE_RECOVER;
      break;

    case CMD_SW_ERR:
      cmd->chassis.mode = CHASSIS_MODE_RELAX;
      break;
  }
  switch (rc->sw_r) {
      /* 右拨杆相应行为选择和解析*/ 
    case CMD_SW_UP:
      cmd->chassis.action = CHASSIS_ACTION_NONE;
      break;

    case CMD_SW_MID:
      cmd->chassis.action = CHASSIS_ACTION_NONE;
      break;

    case CMD_SW_DOWN:
      cmd->chassis.action = CHASSIS_ACTION_NONE;
      break;

    case CMD_SW_ERR:
      cmd->chassis.action = CHASSIS_ACTION_NONE;
      break;
  }
  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
  cmd->chassis.ctrl_vec.vx = rc->ch_l_y;
  cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
  cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
  cmd->chassis.delta_hight = rc->ch_res;

}

// /**
//  * @brief rc失控时机器人恢复放松模式
//  *
//  * @param cmd 主结构体
//  */
static void CMD_RcLostLogic(CMD_t *cmd) {
  /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
  cmd->chassis.mode = CHASSIS_MODE_RELAX;

}

// /**
//  * @brief 初始化命令解析
//  *
//  * @param cmd 主结构体
//  * @param param 参数
//  * @return int8_t 0对应没有错误
//  */
int8_t CMD_Init(CMD_t *cmd) {
  /* 指针检测 */
  if (cmd == NULL) return -1;

  return 0;
}

// /**
//  * @brief 检查是否启用上位机控制指令覆盖
//  *
//  * @param cmd 主结构体
//  * @return true 启用
//  * @return false 不启用
//  */
// inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }

/**
 * @brief 解析命令
 *
 * @param rc 遥控器数据
 * @param cmd 命令
 * @param dt_sec 两次解析的间隔
 * @return int8_t 0对应没有错误
 */
int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  /* 指针检测 */
  if (rc == NULL) return -1;
  if (cmd == NULL) return -1;

  /*c当rc丢控时，恢复机器人至默认状态 */
  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
    CMD_RcLostLogic(cmd);
  } else {
    // if (cmd->pc_ctrl) {
    //   CMD_PcLogic(rc, cmd, dt_sec);
    // } else {
    //   CMD_RcLogic(rc, cmd, dt_sec);
    // }
    CMD_RcLogic(rc, cmd, dt_sec);
  }
  return 0;
}

// /**
//  * @brief 解析上位机命令
//  *
//  * @param host host数据
//  * @param cmd 命令
//  * @param dt_sec 两次解析的间隔
//  * @return int8_t 0对应没有错误
//  */
// int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
//   (void)dt_sec; /* 未使用dt_sec，消除警告 */
//   /* 指针检测 */
//   if (host == NULL) return -1;
//   if (cmd == NULL) return -1;


//   return 0;
// }