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

*/



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

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


/*nuc数据统一到cmd*/
int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
  if(cmd == NULL) return -1;
  if(n == NULL) return -1;

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

			case PICK :
				cmd ->NAVI_t .pick .posx =n->pick .posx ;
				cmd ->NAVI_t .pick .posy =n->pick .posy ;
				cmd ->NAVI_t .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;

  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
     cmd->CMD_CtrlType =RELAXED;
  }	
	else if(rc->sw_l==CMD_SW_UP)
	{
		cmd ->CMD_CtrlType =UP_RCcontrol;
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左上，右上，投篮
		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左上，右中，无模式
	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左上，右上，投篮
	}
	
	else if(rc->sw_l==CMD_SW_MID)
	{
				cmd ->CMD_CtrlType =MID_NAVI;

	}
	else if(rc->sw_l==CMD_SW_DOWN)
	{
		cmd ->CMD_CtrlType =PICK_t;
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Pitch; //左下，右上，投篮
		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Dribble; //左下，右上，投篮
	}	
	
	
	return 0;
}