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

*/



/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"
#include "user_math.h"
#include <string.h>

/* Private function  -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/

int8_t CMD_Init(CMD_t *cmd){
   /*若主结构体为空 自动返回错误 */
  if(cmd == NULL) return-1;
 /**/
  cmd->CMD_CtrlType =RCcontrol;
	cmd->CMD_mode =Normal;

return 0;
}


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



	cmd->Vx = rc->dr16.ch_r_x;  
  cmd->Vy = rc->dr16.ch_r_y;  
  cmd->Vw = rc->dr16.ch_l_x;

  cmd->poscamear = rc->dr16.ch_l_y;

  cmd->key_ctrl_l = rc->dr16.sw_l;
  cmd->key_ctrl_r = rc->dr16.sw_r ;

	
}



/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->CMD_CtrlType = 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->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.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->cmd_MID360.posx = n->navi.vx;
			cmd->cmd_MID360.posy = n->navi.vy;
			cmd->cmd_MID360.posw = n->navi.wz;
			break;
			
			case PICK :
				cmd ->cmd_pick .posx =n->pick .posx ;
				cmd ->cmd_pick .posy =n->pick .posy ;
				cmd ->cmd_pick .posw =n->pick .posw ;

			break;
						

		}
        return 0;
}




/*遥控器，上下层通用，按键控制，统一到cmd*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
	

 if(cmd == NULL) return -1;

	cmd->Vx = rc->dr16.ch_r_x;  
  cmd->Vy = rc->dr16.ch_r_y;  
  cmd->Vw = rc->dr16.ch_l_x;

  cmd->poscamear = rc->dr16.ch_l_y;

  cmd->key_ctrl_l = rc->dr16.sw_l;
  cmd->key_ctrl_r = rc->dr16.sw_r ;
	
  if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) {
     cmd->CMD_CtrlType =RELAXED;
  }	
	else if(rc->dr16.sw_l==CMD_SW_UP)
	{
		cmd ->CMD_CtrlType =RCcontrol;
		if(rc->dr16.sw_r ==CMD_SW_UP)  cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的
		
		if(rc->dr16.sw_r ==CMD_SW_MID)  cmd ->CMD_mode =Normal; //左上，右中，无模式
		
	  if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，手动调整
	}
	
	else if(rc->dr16.sw_l==CMD_SW_MID)
	{
		
		cmd ->CMD_CtrlType =AUTO;		
		if(rc->dr16.sw_r ==CMD_SW_UP) 	cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达	
		
		if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式	
		
	  if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉
		
	}
	else if(rc->dr16.sw_l==CMD_SW_DOWN)
	{
		cmd ->CMD_CtrlType =RCcontrol;
		if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮
		if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
	  if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
	}	
    

	return 0;
}