/*
  控制命令
*/

#include "cmd.h"

#include <string.h>

#include <device\ai.h>
/**
 * @brief 行为转换为对应按键
 *
 * @param cmd 主结构体
 * @param behavior 行为
 * @return uint16_t 行为对应的按键
 */
static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
                                               CMD_Behavior_t behavior) {
  return cmd->param->map.key_map[behavior].key;
}

static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
                                                    CMD_Behavior_t behavior) {
  return cmd->param->map.key_map[behavior].active;
}

/**
 * @brief 检查按键是否按下
 *
 * @param rc 遥控器数据
 * @param key 按键名称
 * @param stateful 是否为状态切换按键
 * @return true 按下
 * @return false 未按下
 */
static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
  /* 按下按键为鼠标左、右键 */
  if (key == CMD_L_CLICK) {
    return rc->mouse.l_click;
  }
  if (key == CMD_R_CLICK) {
    return rc->mouse.r_click;
  }
  return rc->key & (1u << key);
}

static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
                                   CMD_Behavior_t behavior) {
  CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
  CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);

  bool now_key_pressed, last_key_pressed;

  /* 按下按键为鼠标左、右键 */
  if (key == CMD_L_CLICK) {
    now_key_pressed = rc->mouse.l_click;
    last_key_pressed = cmd->mouse_last.l_click;
  } else if (key == CMD_R_CLICK) {
    now_key_pressed = rc->mouse.r_click;
    last_key_pressed = cmd->mouse_last.r_click;
  } else {
    now_key_pressed = rc->key & (1u << key);
    last_key_pressed = cmd->key_last & (1u << key);
  }

  switch (active) {
    case CMD_ACTIVE_PRESSING:
      return now_key_pressed && !last_key_pressed;
    case CMD_ACTIVE_RASING:
      return !now_key_pressed && last_key_pressed;
    case CMD_ACTIVE_PRESSED:
      return now_key_pressed;
  }
}

/**
 * @brief 解析pc行为逻辑
 *
 * @param rc 遥控器数据
 * @param cmd 主结构体
 * @param dt_sec 两次解析的间隔
 */
static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;

  /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
  cmd->gimbal.delta_eulr.yaw =
      (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse;
  cmd->gimbal.delta_eulr.pit =
      (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse;
  cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f;
  cmd->shoot.reverse_trig = false;

  /* 按键行为映射相关逻辑 */
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
    cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
    cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
    cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
    cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
    /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = true;
  } else {
    /* 切换至准备模式，停止射击 */
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = false;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
    /* 每按一次依次切换开火下一个模式 */
    cmd->shoot.fire_mode++;
    cmd->shoot.fire_mode %= FIRE_MODE_NUM;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
    /* 切换到小陀螺模式 */
    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
    cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
    /* 每按一次开、关弹舱盖 */
    cmd->shoot.cover_open = !cmd->shoot.cover_open;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
      /* 停止ai的打符模式，停用host控制 */
      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 {
      /* ai切换至打符模式，启用host控制 */
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
      cmd->ai_status = AI_STATUS_HITSWITCH;
      cmd->host_overwrite = true;
    }
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
      /* 停止ai的自瞄模式，停用host控制 */
      cmd->host_overwrite = false;
      cmd->ai_status = AI_STATUS_STOP;
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
    } else {
      /* ai切换至自瞄模式，启用host控制 */
      cmd->ai_status = AI_STATUS_AUTOAIM;
      cmd->host_overwrite = true;
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
    }
  } else {
    cmd->host_overwrite = false;
    // TODO: 修复逻辑
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
    /* 按下拨弹反转 */
    cmd->shoot.reverse_trig = true;
  }
  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
  }
  /* 保存当前按下的键位状态 */
  cmd->key_last = rc->key;
  memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last));
}

/**
 * @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_BREAK;
      // cmd->host_overwrite = false;
      // cmd->shoot.ai_fire = false;
      break;

    case CMD_SW_MID:
      cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
      // cmd->host_overwrite = true;
      // cmd->shoot.ai_fire = false;
      break;

    case CMD_SW_DOWN:
      cmd->chassis.mode = CHASSIS_MODE_ROTOR;
      cmd->chassis.mode_rotor = ROTOR_MODE_CW;
      // cmd->host_overwrite = true;
      // cmd->shoot.ai_fire = true;
      break;

    case CMD_SW_ERR:
      cmd->chassis.mode = CHASSIS_MODE_RELAX;
      // cmd->host_overwrite = false;
      // cmd->shoot.ai_fire = false;
      break;
  }
  switch (rc->sw_r) {
      /* 右拨杆相应行为选择和解析*/ 
    case CMD_SW_UP:
      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
      cmd->shoot.mode = SHOOT_MODE_SAFE;
      break;

    case CMD_SW_MID:
      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
      cmd->shoot.fire = false;
      cmd->shoot.mode = SHOOT_MODE_LOADED;
      break;

    case CMD_SW_DOWN:
      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
      cmd->shoot.mode = SHOOT_MODE_LOADED;
      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
      cmd->shoot.fire = true;
      break;
		/*
		    case CMD_SW_UP:
      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
      cmd->shoot.mode = SHOOT_MODE_SAFE;
      break;

    case CMD_SW_MID:
      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
      cmd->shoot.fire = false;
      cmd->shoot.mode = SHOOT_MODE_LOADED;
      break;

    case CMD_SW_DOWN:
      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
      cmd->shoot.mode = SHOOT_MODE_LOADED;
      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
      cmd->shoot.fire = true;
      break;
		*/
    case CMD_SW_ERR:
      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
      cmd->shoot.mode = SHOOT_MODE_RELAX;
      // cmd->host_overwrite = false;
  }
  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
  cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
  cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
  cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
  cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}

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

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

  /* 设置机器人的命令参数，初始化控制方式为rc控制 */
  cmd->pc_ctrl = false;
  cmd->param = param;

  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;

  /* 在pc控制和rc控制间切换 */
  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q))
    cmd->pc_ctrl = true;

  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
    cmd->pc_ctrl = false;
  /*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);
    }
  }
  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;

  /* 云台欧拉角设置为host相应的变化的欧拉角 */
  cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
  cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;

  /* host射击命令，设置不同的射击频率和弹丸初速度 */
  if (host->fire) {
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = true;
  } else {
    cmd->shoot.mode = SHOOT_MODE_SAFE;
  }
  return 0;
}

/**
 * @brief 添加向Referee发送的命令
 *
 * @param ref 命令队列
 * @param cmd 要添加的命令
 * @return int8_t 0对应没有错误
 */
int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
  /* 指针检测 */
  if (ref == NULL) return -1;
  /* 越界检测 */
  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;

  /* 添加机器人当前行为状态到画图的命令队列中 */
  ref->cmd[ref->counter] = cmd;
  ref->counter++;
  return 0;
}