/* Includes ----------------------------------------------------------------- */
#include "at9s_pro_cmd.h"

#include <string.h>
#include <math.h>
/* Private define ----------------------------------------------------------- */
#define DEVICE_AT9S_CH_VALUE_MIN  364u
#define DEVICE_AT9S_CH_VALUE_MID  1024u
#define DEVICE_AT9S_CH_VALUE_MAX  1684u
#define DEAD_AREA 0.05f
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

/**
 * @brief ????????
 *
 * @param rc ?????????
 * @param dr16_cmd ????
 * @param dt_sec ???��???????
 * @return int8_t 0?????��???
 */
int8_t COMP_AT9S_CMD_ParseRc(const DEVICE_AT9S_t *rc, COMP_AT9S_CMD_t *at9s_cmd) {
  /* ????? */
  if (rc == NULL) return -1;
  if (at9s_cmd == NULL) return -1;

	at9s_cmd->ch_l_x = 2.0f*(rc->data.ch_l_x-209.0f)/(1596.0f-209.0f)-1.0f;
	at9s_cmd->ch_l_y = 2.0f*(rc->data.ch_l_y-178.0f)/(1575.0f-198.0f)-1.0f;
	at9s_cmd->ch_r_x = 2.0f*(rc->data.ch_r_x-210.0f)/(1596.0f-210.0f)-1.0f;
	at9s_cmd->ch_r_y = 2.0f*(rc->data.ch_r_y-221.0f)/(1604.0f-217.0f)-1.0f;
	
	if(fabs(at9s_cmd->ch_l_x)<=DEAD_AREA)at9s_cmd->ch_l_x = 0;
	if(fabs(at9s_cmd->ch_l_y)<=DEAD_AREA)at9s_cmd->ch_l_y = 0;
	if(fabs(at9s_cmd->ch_r_x)<=DEAD_AREA)at9s_cmd->ch_r_x = 0;
	if(fabs(at9s_cmd->ch_r_y)<=DEAD_AREA)at9s_cmd->ch_r_y= 0;
	
	if(fabs(at9s_cmd->ch_l_x)>=1.0f)at9s_cmd->ch_l_x= at9s_cmd->ch_l_x>0?1.0f:-1.0f;
	if(fabs(at9s_cmd->ch_l_y)>=1.0f)at9s_cmd->ch_l_y= at9s_cmd->ch_l_y>0?1.0f:-1.0f;
	if(fabs(at9s_cmd->ch_r_x)>=1.0f)at9s_cmd->ch_r_x= at9s_cmd->ch_r_x>0?1.0f:-1.0f;
	if(fabs(at9s_cmd->ch_r_y)>=1.0f)at9s_cmd->ch_r_y= at9s_cmd->ch_r_y>0?1.0f:-1.0f;
	
	
	at9s_cmd->chassis_ctrl_vec.x = at9s_cmd->ch_r_x;
	at9s_cmd->chassis_ctrl_vec.y = at9s_cmd->ch_r_y;
	at9s_cmd->gimbal_yaw_ctrl_vec.w = at9s_cmd->ch_l_x;
	at9s_cmd->gimbal_pit_ctrl_vec.w = at9s_cmd->ch_l_y;

	if(rc->online == false ||rc->data.key_H == AT9S_CMD_SW_UP)at9s_cmd->mode = MODE_CMD_BREAKING_CONTROL;
	else if(rc->data.key_E == AT9S_CMD_SW_DOWN)at9s_cmd->mode = MODE_CMD_ONLT_GIMBAL;
	else if(rc->data.key_E == AT9S_CMD_SW_MID)at9s_cmd->mode = MODE_CMD_FOLLOW;
	else if(rc->data.key_E == AT9S_CMD_SW_UP)at9s_cmd->mode = MODE_CMD_SPIN;
  
  	if(rc->data.key_C == AT9S_CMD_SW_DOWN)at9s_cmd->ai = true;
	else at9s_cmd->ai = false;
	
	if(rc->data.key_C == AT9S_CMD_SW_DOWN) at9s_cmd->shoot.ready  = false;
	else if(rc->data.key_C == AT9S_CMD_SW_MID||rc->data.key_C == AT9S_CMD_SW_UP) at9s_cmd->shoot.ready  = true;
	
	if(rc->data.key_C == AT9S_CMD_SW_UP)at9s_cmd->shoot.firecmd = true;
	else 								at9s_cmd->shoot.firecmd = false;

	at9s_cmd->online = rc->online;
  return 0;
}