/*
  控制命令
*/

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include <stdbool.h>
#include <stdint.h>

#include "ahrs.h"

typedef enum {
  CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
  CHASSIS_MODE_DAMP, /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
  CHASSIS_MODE_RECOVER, /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
  CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */
  CHASSIS_MODE_FOLLOW, /* 通过闭环控制使车头方向跟随遥控器 */
} CMD_ChassisMode_t;

typedef enum {
  CHASSIS_ACTION_NONE, /* 无动作 */
  CHASSIS_ACTION_STAND, /* 站立动作 */
  CHASSIS_ACTION_WALK, /* 行走动作 */
  CHASSIS_ACTION_NUM,
}CMD_ChassisAction_t;

/* 底盘控制命令 */
typedef struct {
  CMD_ChassisMode_t mode;     /* 底盘运行模式 */
  CMD_ChassisAction_t action; /* 底盘动作 */
  float delta_hight;          /* 底盘高度，单位：米 */
  MoveVector_t ctrl_vec;      /* 底盘控制向量 */
} CMD_ChassisCmd_t;

typedef struct {
  AHRS_Eulr_t delta_eulr; /* 云台欧拉角变化量 */
} CMD_BodyCmd_t;

/* 拨杆位置 */
typedef enum {
  CMD_SW_ERR = 0,
  CMD_SW_UP = 1,
  CMD_SW_MID = 3,
  CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;


/* 位移灵敏度参数 */
typedef struct {
  float move_sense;      /* 移动灵敏度 */
  float move_fast_sense; /* 加速灵敏度 */
  float move_slow_sense; /* 减速灵敏度 */
} CMD_Move_Params_t;


/* AI行为状态 */
typedef enum {
  AI_STATUS_STOP,      /* 停止状态 */
  AI_STATUS_AUTOMATIC  /* 自动状态 */
} CMD_AI_Status_t;


typedef struct {
  bool host_overwrite; /* 是否Host控制 */

  CMD_AI_Status_t ai_status; /* AI状态 */

  CMD_ChassisCmd_t chassis; /* 底盘控制命令 */
  CMD_BodyCmd_t body; /* 云台控制命令 */
} CMD_t;


typedef struct {
  float ch_r_x; /* 遥控器左侧摇杆横轴值，上为正 */
  float ch_r_y; /* 遥控器左侧摇杆纵轴值，右为正 */
  float ch_l_x; /* 遥控器右侧摇杆横轴值，上为正 */
  float ch_l_y; /* 遥控器右侧摇杆纵轴值，右为正 */

  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */
  
  float ch_res; /* 第五通道值 */

} CMD_RC_t;



typedef struct {
  AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */

  struct {
    float vx;         /* x轴移动速度 */
    float vy;         /* y轴移动速度 */
    float wz;         /* z轴转动速度 */
  } chassis_move_vec; /* 底盘移动向量 */

  bool fire; /* 开火状态 */
} CMD_Host_t;

/**
 * @brief 解析行为命令
 *
 * @param rc 遥控器数据
 * @param cmd 主结构体
 */
int8_t CMD_Init(CMD_t *cmd);

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

/**
 * @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);

// /**
//  * @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);

#ifdef __cplusplus
}
#endif