/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务

*/



/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"

#include <string.h>

/* Private function  -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd){
   /*若主结构体为空 自动返回错误 */
  if(cmd == NULL) return-1;
 /**/
  cmd->C_cmd.type =RC;
	cmd->C_cmd.mode =NORMAL;

return 0;
}


static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd) {

  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
	
  cmd->Vx = rc->ch_r_x;  
  cmd->Vy = -rc->ch_r_y;  
  cmd->Vw = rc->ch_l_x;

  cmd->poscamear = rc->ch_l_y;

  cmd->key_ctrl_l = rc->sw_l;
  cmd->key_ctrl_r = rc->sw_r ;
  
}



/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->C_cmd.mode = RELAXED;
}
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){   
   if (cmd == NULL) return -1;
   if (rc == NULL) return -1;
  /*c当rc丢控时，恢复机器人至默认状态 */
  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
    CMD_RcLostLogic(cmd);
  } else {
      CMD_RcLogic(rc, cmd);
  }
return 0;
}



int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
  if(cmd == NULL) return -1;
  if(n == NULL) return -1;

  cmd->cmd_status = n->status_fromnuc;
  cmd->raw_status = n->ctrl_status;
	
	 for (int i = 0; i < 7; ++i) 
	  { // 从最低位到最高位遍历
        cmd->status[i] = ((cmd->raw_status) & (1 << i)) ? 1 : 0;
    }
    switch(cmd->cmd_status){

			case MID:
			cmd->C_navi.vx = n->navi.vx;
			cmd->C_navi.vy = n->navi.vy;
			cmd->C_navi.wz = n->navi.wz;
			break;
			

		}
        return 0;
}



/*
             遥控器模式重新分配

  这一部分没有设置具体的模式名，后期根据需要修改
		遥控器模式(RC):
		左按键  ---  右按键
		             mode1
          up     	 no_mode	
                     mode2


		             mode3
          down     	 no_mode	
                     mode4

          mid        auto_navi(0x09)雷达导航

*/
int8_t CMD_CtrlSet(CMD_t *cmd) {

 if(cmd == NULL) return -1;
  if(cmd->key_ctrl_l == CMD_SW_UP)//当左拨杆打到最上面时 强制使用遥控器控制
	{
       /*遥控器模式下，右按键三种状态分配*/	

		if(cmd->key_ctrl_r==CMD_SW_UP)
		{
		     cmd->C_cmd.type = RC;
		     cmd->C_cmd.mode = RC_MODE1;			
		}
		if(cmd->key_ctrl_r==CMD_SW_MID)
		{
		    cmd->C_cmd.type = RC;
		    cmd->C_cmd.mode = RC_NO_MODE;			
		}		
		if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
		{
		     cmd->C_cmd.type = RC;
		     cmd->C_cmd.mode =RC_MODE2;			
		}
		
	}
	else if(cmd->key_ctrl_l ==CMD_SW_DOWN)
	{
		if(cmd->key_ctrl_r==CMD_SW_UP)
		{
		     cmd->C_cmd.type = RC;
		     cmd->C_cmd.mode = RC_MODE3;			
		}
		if(cmd->key_ctrl_r==CMD_SW_MID)
		{
		    cmd->C_cmd.type = RC;
		    cmd->C_cmd.mode = RC_NO_MODE;			
		}		
		if(cmd->key_ctrl_r==CMD_SW_DOWN)//遥控器控制模式下，右拨杆上时启动
		{
		     cmd->C_cmd.type = RC;
		     cmd->C_cmd.mode = RC_MODE4;			
		}
		
	}
	else //左按键打到中间，自动模式
	{	
      if(	cmd->key_ctrl_l==CMD_SW_MID	){
		 cmd->C_cmd.type = MID_NAVI; 
 		    cmd->C_cmd.mode = RC_NO_MODE;			
          		 
      switch(cmd->cmd_status)
{
	  case MID://雷达，视觉导航
		 cmd->C_cmd.type = MID_NAVI; 
	   break;
  }
 }
 
  }
return 0;
}
//接收码盘导航的返回数据，传入cmd_t结构体
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
{
	 if(cmd == NULL) return -1;
   if(act == NULL) return -1;
	 
	if(cmd->key_ctrl_l ==CMD_SW_MID	)
	{
		if(cmd->key_ctrl_r == CMD_SW_UP) 
			act->flag =1;
		
		if(cmd->key_ctrl_r == CMD_SW_DOWN ) 
			act->flag =-1;
				
	}
	
	 cmd->C_navi .vx  =act ->out .Vx ;
	 cmd ->C_navi .vy  =act ->out .Vy ;
	 cmd ->C_navi .wz  =act ->out .Vw ;

	 
	 return 0;
	
}