#ifndef _CMD_H
#define _CMD_H
#include "struct_typedef.h"
#include "device.h"
#include "cmd.h"
#include "gpio.h"
#include "user_math.h"
#include <string.h>

#define MID (0x09)
#define PICK (0x06)

typedef enum{
	RCcontrol,//遥控器控制，左按键上，控制上层
	AUTO,
	RELAXED,//异常模式

 }CMD_CtrlType_t;

typedef  enum{
			
     
		Normal,    //无模式
	
	  AUTO_NAVI,
	  AUTO_PICK,
	
	  UP_Adjust,//上层调整

    Dribble ,  //运球
	  Pitch,     //投篮,底盘锁定
	
	  
    Dribbl_transfer
  }CMD_mode_t;
typedef struct {
 uint8_t  status_fromnuc;
 uint8_t  ctrl_status;  //取其中每一个二进制位用作通信
  struct 
  {
  fp32 vx;
  fp32 vy;
  fp32 wz;
  }navi;
 struct 
 {
	 fp32 posx;
   fp32 posy;
   fp32 posw;
 }pick;
  struct
	{
		fp32 angle;
	}sick_cali;
} CMD_NUC_t;
/* 拨杆位置 */
typedef enum {
  CMD_SW_ERR = 0,
  CMD_SW_UP = 1,
  CMD_SW_MID = 3,
  CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;


typedef enum{
	RC_DR16,
	RC_LD	,
	Control_loss

}rc_type_t;
/*遥控器值，使用CMD_RcLogic函数传入CMD_t结构体*/
typedef struct {
	
	rc_type_t rc_type;
	
	struct {
  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
  float ch_res; /* 第五通道值 */
  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */

  uint16_t key; /* 按键值 */

  uint16_t res; /* 保留，未启用 */
	}__attribute__((packed))dr16;
	
	struct {
  fp32 ch_r_x ;
  fp32 ch_r_y ;
  fp32 ch_l_x ;
  fp32 ch_l_y ;
  CMD_SwitchPos_t key_A ;
//  int16_t key_B ;//��
  CMD_SwitchPos_t key_C ;
//  int16_t key_D ;//��
  CMD_SwitchPos_t key_E ;
  CMD_SwitchPos_t key_F;
  CMD_SwitchPos_t key_G ;
  CMD_SwitchPos_t key_H ;
	
	int16_t knob_left;//左旋钮
	int16_t knob_right;//右旋钮

		
	}__attribute__((packed))LD;


} __attribute__((packed))CMD_RC_t;

typedef struct {
 
   CMD_mode_t CMD_mode;
   CMD_CtrlType_t CMD_CtrlType;
	
   uint8_t cmd_status;
   uint8_t raw_status;	 
   uint8_t status[7];	
	
   fp32 key_ctrl_l;
   fp32 key_ctrl_r;
	
	 fp32 Vx;
   fp32 Vy;
   fp32 Vw;
   fp32 poscamear;

		struct 
		{
			 fp32 posx;
			 fp32 posy;
			 fp32 posw;
			}cmd_pick;
				struct 
		{
			 fp32 posx;
			 fp32 posy;
			 fp32 posw;
			}cmd_MID360;
		 


	
} CMD_t;




int8_t CMD_Init(CMD_t *cmd);


static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) ;
static void CMD_RcLostLogic(CMD_t *cmd);
int8_t CMD_ParseRc(CMD_t *cmd,const CMD_RC_t *rc); 

int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);
int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) ;
int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc);

#endif