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

*/

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

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

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

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;
}
void 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:
		 if (rc->LD.key_D == 0) {
        CMD_RcLostLogic(cmd);
      } 
		 else{
		 /* 上电保护检查 */			 
//			if(!cmd->cmd_power_on_safe){
//		    if (rc->LD.key_A == CMD_SW_DOWN && rc->LD.key_D == CMD_SW_MID && rc->LD.key_E == CMD_SW_MID){
//          cmd->cmd_power_on_safe = 1;
//        }
//				else{
//				  // 否则保持停止状态
//          cmd->C_cmd.mode = STOP;
//				}      
//     }
//		 else{
	     CMD_remote(rc, cmd);
//		 } 			 
		}
   break;

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

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;
	} 
 }		
  //乐迪
 else if(rc->rc_type==RC_LD){

//模式切换	 
  switch(rc->LD.key_D){
	 case CMD_SW_UP:
		cmd->C_cmd.mode = NAVI; 
		break;
	 case CMD_SW_DOWN:
		cmd->C_cmd.mode = RC;
		break;
	 case CMD_SW_MID:
		cmd->C_cmd.mode = STOP;
	}
//导航切换 
	 switch(rc->LD.key_E){
    case CMD_SW_UP:
			cmd->C_cmd.nuc_radar = ANGLE; 
			break;			
		case CMD_SW_MID:
		  cmd->C_cmd.nuc_radar = WORLD;
			break;
		case CMD_SW_DOWN:
			cmd->C_cmd.nuc_radar = RADAR_RESET;     					
	}	
//锁定切换	
	switch(rc->LD.key_A){
    case CMD_SW_UP:
			cmd->C_cmd.communicate = YES; 
			break;			
		case CMD_SW_DOWN:
			cmd->C_cmd.communicate = NO;     			
	}
	
//点位切换				
static int previous_pos = -1; 

if(rc->LD.knob_left > 199 && rc->LD.knob_left < 265){ //200
    if(cmd->C_cmd.pos != POS_1) {
        if(previous_pos != POS_1) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_1;
        previous_pos = POS_1;
    }
}     
else if(rc->LD.knob_left > 265 && rc->LD.knob_left < 405){//330
    if(cmd->C_cmd.pos != POS_2) {
        if(previous_pos != POS_2) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_2;
        previous_pos = POS_2;
    }
}
else if(rc->LD.knob_left > 405 && rc->LD.knob_left < 554){  //479
    if(cmd->C_cmd.pos != POS_3) {
        if(previous_pos != POS_3) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_3;
        previous_pos = POS_3;
    }
}
else if(rc->LD.knob_left > 554 && rc->LD.knob_left < 704){ //629
    if(cmd->C_cmd.pos != POS_4) {
        if(previous_pos != POS_4) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_4;
        previous_pos = POS_4;
    }
}
else if(rc->LD.knob_left > 704 && rc->LD.knob_left < 853){//778
    if(cmd->C_cmd.pos != POS_5) {
        if(previous_pos != POS_5) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_5;
        previous_pos = POS_5;
    }
}
else if(rc->LD.knob_left > 853 && rc->LD.knob_left < 1002){//928
    if(cmd->C_cmd.pos != POS_6) {
        if(previous_pos != POS_6) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_6;
        previous_pos = POS_6;
    }
}
else if(rc->LD.knob_left > 1002 && rc->LD.knob_left < 1150){ //1075
    if(cmd->C_cmd.pos != POS_7) {
        if(previous_pos != POS_7) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_7;
        previous_pos = POS_7;
    }
}
else if(rc->LD.knob_left > 1150 && rc->LD.knob_left < 1300){ //1224
    if(cmd->C_cmd.pos != POS_8) {
        if(previous_pos != POS_8) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_8;
        previous_pos = POS_8;
    }
}
else if(rc->LD.knob_left > 1300 && rc->LD.knob_left < 1448){ //1373
    if(cmd->C_cmd.pos != POS_9) {
        if(previous_pos != POS_9) cmd->C_cmd.dribble = RUNNING;
        cmd->C_cmd.pos = POS_9;
        previous_pos = POS_9;
    }
}
//运球时锁死底盘
//  if(rc->LD.key_G == CMD_SW_DOWN&&cmd->C_cmd.pos != POS_1){
//		cmd->C_cmd.dribble = Pause; 
//  }	
 }	
 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){

#ifdef nuc_1
	   cmd->nuc.flag =  n->nuc.flag;
	   cmd->nuc.angle = n->nuc.angle;
	   cmd->nuc.yaw = n->nuc.yaw;		 
#elif defined(nuc_2)
	   cmd->nuc.angle = n->nuc.angle;
	   cmd->nuc.yaw = n->nuc.yaw;		
#endif		   	

	return 0;
}
