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

*/



/* 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;
  const  CMD_CtrlType_t  prev_CtrlType = cmd->CMD_CtrlType ;
	const  CMD_UP_mode_t   prev_mode     = cmd->CMD_UP_mode ;//保存旧状态
	
  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 =Normal; //左上，右上，运球
	}
	
	else if(rc->sw_l==CMD_SW_MID)
	{
				
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_CtrlType =MID_NAVI;; //左中，右上，雷达
		if(rc->sw_r ==CMD_SW_MID) 
		{
		  cmd ->CMD_CtrlType =UP_RCcontrol;
      cmd ->CMD_UP_mode =Normal; //左中，右中，无模式	
		}
	  if(rc->sw_r ==CMD_SW_DOWN) 
		{
		cmd ->CMD_UP_mode =Normal; //左中，右下，无模式
	  cmd ->CMD_CtrlType =UP_RCcontrol;
		}
	}
	else if(rc->sw_l==CMD_SW_DOWN)
	{
		cmd ->CMD_CtrlType =UP_RCcontrol;
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_UP_mode =Normal; //左下，右上，投篮
		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_UP_mode =Normal; //左下，右中，无模式
	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_UP_mode =Normal; //左下，右上，无模式
	}	
    
   if (prev_CtrlType!= cmd->CMD_CtrlType ) cmd ->last_CMD_CtrlType = prev_CtrlType;
	 if (prev_mode!=cmd->CMD_UP_mode  ) cmd ->last_CMD_UP_mode  =prev_mode;


	return 0;
}