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

#include <string.h>


//#include "error_detect.h"
#include  "bsp/uart.h"
#include "module/cmd.h"
#include "bsp/calc_lib.h"

#define R12DS
//#define DR16
/*
两个遥控器都是
             x                
             |                      
             |                     
              ———y 
*/
#ifdef DR16

#define FRAME_LEN 36

#endif

#ifdef R12DS

#define FRAME_LEN 25u

#endif
/* Private define ----------------------------------------------------------- */
#define REMOTE_CH_VALUE_MIN (364u)
#define REMOTE_CH_VALUE_MID (1024u)
#define REMOTE_CH_VALUE_MAX (1684u)

/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */


static osThreadId_t thread_alert;
//uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
//uint8_t cbuf[sizeof(LD_raw_t)];
uint8_t cbuf[25u];
/* Private function  -------------------------------------------------------- */
int8_t REMOTE_Restart(void) {
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
  return DEVICE_OK;
}

static void REMOTE_RxCpltCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
// 	detect_hook(DR16_TOE);
}
//遥控器初始化
int8_t REMOTE_Init(void) {
  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  BSP_UART_RegisterCallback(BSP_UART_DR16,BSP_UART_RX_CPLT_CB,
                           REMOTE_RxCpltCallback);
  memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包                          
  return DEVICE_OK;
}

int8_t REMOTE_StartDmaRecv(void) {
  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
                           (uint8_t *)cbuf,
                           sizeof(cbuf)) == HAL_OK)
//	  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
//                           (uint8_t *)cbuf,
//                           sizeof(cbuf)) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}

bool REMOTE_WaitDmaCplt(uint32_t timeout) {
  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
          SIGNAL_DR16_RAW_REDY);
}
/*大疆数据解析 */
static bool DR16_DataCorrupted(const DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;

  if ((dr16->data.ch_r_x < REMOTE_CH_VALUE_MIN) ||
      (dr16->data.ch_r_x > REMOTE_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_r_y < REMOTE_CH_VALUE_MIN) ||
      (dr16->data.ch_r_y > REMOTE_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_x < REMOTE_CH_VALUE_MIN) ||
      (dr16->data.ch_l_x > REMOTE_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_y < REMOTE_CH_VALUE_MIN) ||
      (dr16->data.ch_l_y > REMOTE_CH_VALUE_MAX))
    return true;

  if (dr16->data.sw_l == 0) return true;

  if (dr16->data.sw_r == 0) return true;

  return false;
}

int8_t DR16_ParseRaw(DR16_t *dr16){
	if(dr16 ==NULL) return DEVICE_ERR_NULL;
	 dr16->data.ch_r_x = (cbuf[0] | (cbuf[1] <<8));
	 dr16->data.ch_r_y = ((cbuf[1] >> 3)| (cbuf[2] << 5));
	 dr16->data.ch_l_x = ((cbuf[2] >>6) | (cbuf[3] << 2) | (cbuf[4] <<10));
	 dr16->data.ch_l_y = ((cbuf[4] >>1) | (cbuf[5] <<7));
	 dr16->data.sw_r = ((cbuf[5] >>4));
   dr16->data.sw_l = ((cbuf[5] >> 4) & 0x000C) >> 2;             
	
	return 1;
}
/*乐迪数据解析 */
/*乐迪用的按键封装 */
CMD_SwitchPos_t Keymap(int16_t value) {
  return (value == 353) ? CMD_SW_UP :
         (value == 1024) ? CMD_SW_MID :
         (value == 1695) ? CMD_SW_DOWN : CMD_SW_ERR;
}

int8_t ET16s_ParseRaw( ET16s_raw_t *raw){		
//ET16s
	raw->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff;        //Channel 1
  raw->ch[3] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //Channel 2
  raw->ch[1] = ((cbuf[3] >> 6) | (cbuf[4] << 2) |          //Channel 3
                       (cbuf[5] << 10)) &0x07ff;
  raw->ch[2] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //Channel 4

  raw->sw[6] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF;                   								//Channel 5
  raw->sw[0] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF;    	//Channel 6
  raw->sw[5] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;;                    							//Channel 7
  raw->sw[4] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF;                    							//Channel 8
  raw->sw[3] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF;                  								//Channel 9
  raw->sw[2] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF;                               	//Channel 10
  raw->sw[1] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF;	//Channel 11
  raw->sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;  
	
  raw->ch[1] = map_fp32(raw->ch[1],353,1695,-1,1);		//lx
	raw->ch[0] = map_fp32(raw->ch[0],353,1695,-1,1);		//ly
	raw->ch[2] = map_fp32(raw->ch[2],353,1695,-1,1);		//rx
	raw->ch[3] = map_fp32(raw->ch[3],353,1695,-1,1);     //ry
	
	if(raw->ch[0]>-0.01&&raw->ch[0]<=0.01) raw->ch[0]=0;              
	if(raw->ch[1]>-0.01&&raw->ch[1]<=0.01) raw->ch[1]=0;                
	if(raw->ch[2]>-0.01&&raw->ch[2]<=0.01) raw->ch[2]=0;              
	if(raw->ch[3]>-0.01&&raw->ch[3]<=0.01) raw->ch[3]=0;       
  return 1;

}

int8_t REMOTE_ParseRC(DR16_t *dr16, CMD_RC_t *rc,ET16s_raw_t *et16s) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;
  if (et16s == NULL) return DEVICE_ERR_NULL;

#ifdef DR16 
  rc->rc_type=RC_DR16; 
  DR16_ParseRaw(dr16);
  if (DR16_DataCorrupted(dr16)) {
		return DEVICE_ERR;
  } else {
    memset(rc, 0, sizeof(*rc));
  }

  float full_range = (float)(REMOTE_CH_VALUE_MAX - REMOTE_CH_VALUE_MIN);

  rc->DJ.ch_r_x = 2 * ((float)dr16->data.ch_r_y - REMOTE_CH_VALUE_MID) / full_range;
  rc->DJ.ch_r_y = 2 * ((float)dr16->data.ch_r_x - REMOTE_CH_VALUE_MID) / full_range;
  rc->DJ.ch_l_x = 2 * ((float)dr16->data.ch_l_y - REMOTE_CH_VALUE_MID) / full_range;
  rc->DJ.ch_l_y = 2 * ((float)dr16->data.ch_l_x - REMOTE_CH_VALUE_MID) / full_range;

  rc->DJ.sw_l = (CMD_SwitchPos_t)dr16->data.sw_l;
  rc->DJ.sw_r = (CMD_SwitchPos_t)dr16->data.sw_r;

 // rc->dr16.key = dr16->data.key;

//  rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;

#elif defined(R12DS)	
	
	/*天地飞*/
	rc->rc_type=RC_ET16s;
	ET16s_ParseRaw(et16s);
	
  rc->ET16s.ch_r_x = et16s->ch[2] ;
  rc->ET16s.ch_r_y = et16s->ch[3] ;
	rc->ET16s.ch_l_x = et16s->ch[0] ;
  rc->ET16s.ch_l_y = et16s->ch[1] ;
// 
//   rc->ET16s.key_A = (ET16s->sw[0]);
//  rc->ET16s.key_B = (ET16s->sw[1]);
//  rc->ET16s.key_C = (ET16s->sw[2]);
//  rc->ET16s.key_D = (ET16s->sw[3]);
//  rc->ET16s.key_E = (ET16s->sw[4]);
//  rc->ET16s.key_F = (ET16s->sw[5]);
//  rc->ET16s.key_G = (ET16s->sw[6]);
//  rc->ET16s.key_H = (ET16s->sw[7]);
  rc->ET16s.key_A = Keymap(et16s->sw[0]);
  rc->ET16s.key_B = Keymap(et16s->sw[1]);
  rc->ET16s.key_C = Keymap(et16s->sw[2]);
  rc->ET16s.key_D = Keymap(et16s->sw[3]);
  rc->ET16s.key_E = Keymap(et16s->sw[4]);
  rc->ET16s.key_F = Keymap(et16s->sw[5]);
  rc->ET16s.key_G = Keymap(et16s->sw[6]);	
  rc->ET16s.key_H = Keymap(et16s->sw[7]);
  // rc->ET16s.knob_left = ET16s->sw[7]; //200 330 479 629 778 928 1075 1224 1373 1522 1670 1800

//  /*离线处理，和dr16位置不同*/
	if(et16s->sw[6]==1695)
	{
		ET16s_HandleOffline(et16s,rc);
//    memset(cbuf, 0, sizeof(cbuf));	//有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信	
	}
#endif
  return DEVICE_OK;
}

int8_t DR16_HandleOffline(const DR16_t *dr16, CMD_RC_t *rc) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;
  if (rc == NULL) return DEVICE_ERR_NULL;

  rc->rc_type =Control_loss ;
  memset(&rc->DJ , 0, sizeof(DR16_t));
  return 0;
}

int8_t ET16s_HandleOffline(const ET16s_raw_t *ET16s, CMD_RC_t *rc) {
	if (ET16s == NULL) return DEVICE_ERR_NULL;
  if (rc == NULL) return DEVICE_ERR_NULL;
  
  rc->rc_type =Control_loss ;
	memset(&rc->ET16s , 0, sizeof(ET16s_raw_t));
	return 0;
}
