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

*/



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

  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
//	    RC_dr16 ,  
//    RC_r12ds  
//	

	/*乐迪反馈值转换(-1  --  1 )范围*/
	
//	if(rc->ch_x<0) cmd->Vx =map_fp32(rc->ch_x,-337,0,-1,0);
//	else cmd->Vx =map_fp32(rc->ch_x,0,310,0,1);
//	
//	
//	if(rc->ch_y<0) cmd->Vy =map_fp32(rc->ch_y,-260,0,-1,0);
//	else cmd->Vy =map_fp32(rc->ch_y,0,312,0,1);

//	if(rc->ch_w<0) cmd->Vw =map_fp32(rc->ch_w,-308,0,-1,0);
//	else cmd->Vw =map_fp32(rc->ch_w,0,291,0,1);


//  cmd->key_ctrl_l = rc->key [0];
//  cmd->key_ctrl_r = rc->key [1];
//	


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

  cmd->poscamear = rc->ch_l_y;

  cmd->key_ctrl_l = rc->sw_l;
  cmd->key_ctrl_r = rc->sw_r ;

	
}



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

  /*c当rc丢控时，恢复机器人至默认状态 */
  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
    CMD_RcLostLogic(cmd);
  } else {
      CMD_RcLogic(rc, cmd);
  }
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;
}



/*
             遥控器模式重新分配

  这一部分没有设置具体的模式名，后期根据需要修改
		遥控器模式(RC):
		左按键  ---  右按键
		                 mode1
          up     	   no_mode	
                     mode2


		                 mode3
          down     	 no_mode	
                     mode4

          mid        auto_navi(0x09)雷达导航

*/
/*遥控器，上下层通用，按键控制，统一到cmd*/
int8_t CMD_ParseRC(CMD_t *cmd,const CMD_RC_t *rc) {
	

 if(cmd == NULL) return -1;

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

  cmd->poscamear = rc->ch_l_y;

  cmd->key_ctrl_l = rc->sw_l;
  cmd->key_ctrl_r = rc->sw_r ;
	
  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 =RCcontrol;
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Pitch; //左上，右上，投篮，设置好的
		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式
	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Pitch_HAND; //左上，右上，手动调整
	}
	
	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 =RCcontrol;
      cmd ->CMD_mode =Normal; //左中，右中，无模式	
		}
	  if(rc->sw_r ==CMD_SW_DOWN) 
		{
		cmd ->CMD_mode =Normal; //左中，右下，无模式
	  cmd ->CMD_CtrlType =RCcontrol;
		}
	}
	else if(rc->sw_l==CMD_SW_DOWN)
	{
		cmd ->CMD_CtrlType =RCcontrol;
		if(rc->sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，投篮
		if(rc->sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左下，右中，无模式
	  if(rc->sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式
	}	
    

	return 0;
}