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

*/

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

/* Private function  -------------------------------------------------------- */
/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd){

  cmd->C_cmd.mode = STOP;
  cmd->cmd_power_on_safe = 0;  // 初始为不安全状态
}

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

  if(rc->rc_type==RC_DR16){

    cmd->Vx = rc->DJ.ch_r_x;  
    cmd->Vy = rc->DJ.ch_r_y;  
    cmd->Vw = rc->DJ.ch_l_y;
    cmd->throttle = rc->DJ.ch_l_x;	
  }
  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_y;
	  cmd->throttle = rc->LD.ch_l_x;
  }
} 	
/**
 * @brief rc失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
static void CMD_RcLostLogic(CMD_t *cmd){
  /* 机器人底盘运行模式恢复至放松模式 */
  cmd->C_cmd.mode = STOP;
}
int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){   
  
  switch (rc->rc_type){
	  case  RC_DR16:
      /*c当rc丢控时，恢复机器人至默认状态 */
      if ((rc->DJ.sw_l == CMD_SW_ERR) || (rc->DJ.sw_r == CMD_SW_ERR)) {
        CMD_RcLostLogic(cmd);
		    cmd->cmd_power_on_safe = 0;
      } 
			else{
		 /* 上电保护检查 */
//		 if(!cmd->cmd_power_on_safe && !pos.Action_ready){
//		    if (rc->sw_l == CMD_SW_MID && pos.Action_ready){
//            cmd->cmd_power_on_safe = 1;
//        }
//				else{
//				  // 否则保持停止状态
//          cmd->C_cmd.mode = STOP;
//				}      
//     }
//		 else{
	       CMD_remote(rc, cmd);
//		 }    
      }
   break;
   case RC_LD:
     CMD_remote(rc, cmd);		
   break;

   case Control_loss:
     CMD_RcLostLogic(cmd);
	 break;
  }
return 0;
}

int8_t CMD_CtrlSet(CMD_t *cmd,const CMD_RC_t *rc){
 
	//大疆
 if(rc->rc_type==RC_DR16){
	 
  switch(rc->DJ.sw_l){
	case CMD_SW_UP:
		cmd->C_cmd.mode = RC; 

		break;
	case CMD_SW_DOWN:
		cmd->C_cmd.mode = NAVI;
		break;
		case CMD_SW_MID:
		 cmd->C_cmd.mode = STOP;
	}
	switch(rc->DJ.sw_r){
		case CMD_SW_UP:
			cmd->C_cmd.pos = POS_1; 
			break;			
		case CMD_SW_MID:
		    cmd->C_cmd.pos = POS_2;
			break;
		case CMD_SW_DOWN:
			cmd->C_cmd.pos = POS_3;
		} 
 }		
  //乐迪
 else if(rc->rc_type==RC_LD){
	 
 switch(rc->LD.key_H){
	case CMD_SW_UP:
		cmd->C_cmd.mode = RC; 

		break;
	case CMD_SW_DOWN:
		cmd->C_cmd.mode = NAVI;
		break;
		case CMD_SW_MID:
		 cmd->C_cmd.mode = STOP;
	}
	switch(rc->LD.key_G){
		case CMD_SW_UP:
			cmd->C_cmd.pos = POS_1; 
			break;			
		case CMD_SW_MID:
		    cmd->C_cmd.pos = POS_2;
			break;
		case CMD_SW_DOWN:
			cmd->C_cmd.pos = POS_3;
		} 
	}	
 
//switch(cmd->cmd_status){
	
//	    cmd->C_cmd.mode = NORMAL;
//	   break;
// }
return 0;
}

//接收码盘导航的返回数据，传入cmd_t结构体
int8_t CMD_ParseAction(CMD_t *cmd,CMD_ACTION_t *act)
{

	 cmd ->C_navi .vx =act ->out .Vx ;
	 cmd ->C_navi .vy =act ->out .Vy ;
	 cmd ->C_navi .wz =act ->out .Vw ;
	 
	 return 0;	
}

int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){

	//    switch(cmd->cmd_status){
	//			case NAVI:
				//  cmd->C_navi.vx = n->navi.vx;
				//  cmd->C_navi.vy = n->navi.vy;
				  cmd->camera_yaw = n->navi.vx;
	//			break;
	//		}
	return 0;
}