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

*/



/* 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) {


		if(rc->rc_type==RC_DR16){
		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->rc_type==RC_LD){

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

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

		}

}



/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->CMD_CtrlType = RELAXED;
}
int8_t CMD_ParseRc(CMD_t *cmd,const CMD_RC_t *rc){   
   if (cmd == NULL) return -1;
   if (rc == NULL) return -1;

		switch (rc->rc_type){
			
			case  RC_DR16:
				CMD_DR16(cmd,rc);
		break ;
			
			case RC_LD:
	      CMD_LD(cmd,rc);		
		break ;
		
		  case Control_loss:
        CMD_RcLostLogic(cmd);
				break ;
		}
  
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;
}




/*DR16遥控器数据分配*/
int8_t CMD_DR16(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;
}

int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
 
 if(cmd == NULL) return -1;

	
	cmd->Vx = rc->LD.ch_r_x;  
  cmd->Vy = rc->LD.ch_r_y;  
  cmd->Vw = rc->LD.ch_l_x;
  cmd->poscamear = rc->LD.ch_l_y;
	
	
	/*自动手动切换*/
	if(rc->LD.key_A ==CMD_SW_DOWN ) cmd ->CMD_CtrlType =RCcontrol;
	else if(rc->LD.key_A ==CMD_SW_UP) cmd ->CMD_CtrlType =AUTO;
	
	
	if(cmd ->CMD_CtrlType ==AUTO){
	
/*自动下的*/	
	if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =AUTO_NAVI; 
	else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_PICK;
	else cmd ->CMD_mode =Normal ;
	
	}
	else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/	
	if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode =UP_Adjust; 
	else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
	else cmd ->CMD_mode =Normal;
	}
}