/*
       遥控
       dr16：dbus       乐迪：s-ubs
			 dr16掉线不输出信号，可以使用超时检测方式判断掉线
			 即RC_SBUS_WaitDmaCplt
			 乐迪掉线依然有信号输出，只是部分通道变为特定值，无法使用以上方式

*/

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

#include <string.h>

#include "bsp_usart.h"
#include "error_detect.h"


#define DR16
//#define LD_t

#ifdef DR16
#define FRAME_LEN 36
#endif
#ifdef LD_t

#define FRAME_LEN 25

#endif
/* Private define ----------------------------------------------------------- */

#define RC_CH_VALUE_MIN         ((uint16_t)364)
#define RC_CH_VALUE_MID         ((uint16_t)1024)
#define RC_CH_VALUE_MAX         ((uint16_t)1684)

static osThreadId_t thread_alert;
 uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况

/*通用，初始化，串口回调注册，dma数据接收*/
int8_t RC_SBUS_Init(void )
{
  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  BSP_UART_RegisterCallback(BSP_UART_REMOTE, BSP_UART_IDLE_LINE_CB,
                            RC_RxCpltCallback);
	memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包
	
  return DEVICE_OK;
}

static void RC_RxCpltCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_RC_RAW_REDY);
//	detect_hook(DR16_TOE);
}
int8_t RC_SBUS_Restart(void) {
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REMOTE));
  return DEVICE_OK;
}
int8_t RC_StartDmaRecv(void) {	
	
  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_REMOTE),
                           (uint8_t *)cbuf,
                           sizeof(cbuf)) == HAL_OK)																		 
    return DEVICE_OK;
    return DEVICE_ERR;
}
bool RC_WaitDmaCplt(uint32_t timeout) {
  return (osThreadFlagsWait(SIGNAL_RC_RAW_REDY, osFlagsWaitAll, timeout) ==
          SIGNAL_RC_RAW_REDY);
}
/*乐迪数据解析 */
int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
{		
		raw->ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff;        //!< Channel 0
    raw->ch[1] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //!< Channel 1
    raw->ch[2] = ((cbuf[3] >> 6) | (cbuf[4] << 2) |          //!< Channel 2
                         (cbuf[5] << 10)) &0x07ff;
    raw->ch[3] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //!< Channel 3
    
		raw->sw[0] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF;                  
    raw->sw[1] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF;                       
    raw->sw[2] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;;                   
    raw->sw[3] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF;                   
    raw->sw[4] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF;                  
    raw->sw[5] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF;                                  
    raw->sw[6] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF;                                 
    raw->sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;                   

		raw->ch[0] -= RC_CH_VALUE_MID;
    raw->ch[1] -= RC_CH_VALUE_MID;
    raw->ch[2] -= RC_CH_VALUE_MID;
    raw->ch[3] -= RC_CH_VALUE_MID;
    

		raw->ch[0] += 24;	//y(-694,693)左右
		raw->ch[1] = -raw->ch[1]-24;	//x(-693,694)前后
		raw->ch[2] = -raw->ch[2]+764;		//m(95,1482)油门
		raw->ch[3] += 24;	//w(-694,693)旋转
		
		raw->ch[1] = map_int(raw->ch[1],-693,694,-700,700);		//x映射到(-700,700)
		raw->ch[0] = map_int(raw->ch[0],-694,693,-700,700);		//y映射到(-700,700)
		raw->ch[3] = 0.5*(raw->ch[3]);		//w
		
		
if (raw->ch[0] < 0)
    raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
else
    raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);

// ch[1]
if (raw->ch[1] < 0)
    raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
else
    raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);

    raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);

// ch[3]
if (raw->ch[3] < 0)
    raw->map_ch[3] = map_fp32(raw->ch[3], -344, 0, -1, 0);
else
    raw->map_ch[3] = map_fp32(raw->ch[3], 0, 348, 0, 1);

		
		/*非线性映射*/
		raw->map_ch[0]=expo_map(raw->map_ch[0],0.7f);
		raw->map_ch[1]=expo_map(raw->map_ch[1],0.7f);
		raw->map_ch[2]=expo_map(raw->map_ch[2],0.7f);
		raw->map_ch[3]=expo_map(raw->map_ch[3],0.7f);
		
		//死区(-30,30)                            
		if(raw->map_ch[0]>-0.002f&&raw->map_ch[0]<0.002f) raw->map_ch[0]=0;
		if(raw->map_ch[1]>-0.002f&&raw->map_ch[1]<0.002f) raw->map_ch[1]=0;
		if(raw->map_ch[2]>-0.002f&&raw->map_ch[2]<0.002f) raw->map_ch[2]=0;
		if(raw->map_ch[3]>-0.002f&&raw->map_ch[3]<0.002f) raw->map_ch[3]=0;
																						

/*dr16采用的是复制内存，这里因为内存顺序不同没用*/
 		LD->ch_l_x=	raw->map_ch[3];
 		LD->ch_l_y =raw->map_ch[2];
 		LD->ch_r_x=	raw->map_ch[0];
 		LD->ch_r_y =raw->map_ch[1];

// 		LD->key_A  =raw->sw[0];
// //		LD->key_B  =rc_ctrl->sw[0]
// 		LD->key_C  =raw->sw[2];
// //		LD->key_D  =rc_ctrl->sw[0]
// 		LD->key_E  =raw->sw[5];
// 		LD->key_F  =raw->sw[3];
// 		LD->key_G  =raw->sw[7];
// 		LD->key_H  =raw->sw[4];
// 		LD->knob_left=raw->sw[1];
// 	  LD->knob_right =raw->sw[6];
		
	  LD->key_A = MapSwitchState(raw->sw[0]);
    LD->key_C = MapSwitchState(raw->sw[2]);
    LD->key_E = MapSwitchState(raw->sw[5]);
    LD->key_F = MapSwitchState(raw->sw[3]);
    LD->key_G = MapSwitchState(raw->sw[7]);
    LD->key_H = MapSwitchState(raw->sw[4]);
    LD->knob_left = MapSwitchState(raw->sw[1]);
    LD->knob_right = MapSwitchState(raw->sw[6]);
		
		
		return 1;

}
/*dr16数据解析+校验 */
static bool DR16_DataCorrupted(const DR16_t *dr16) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;

  if ((dr16->data.ch_r_x < RC_CH_VALUE_MIN) ||
      (dr16->data.ch_r_x > RC_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_r_y < RC_CH_VALUE_MIN) ||
      (dr16->data.ch_r_y > RC_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_x < RC_CH_VALUE_MIN) ||
      (dr16->data.ch_l_x > RC_CH_VALUE_MAX))
    return true;

  if ((dr16->data.ch_l_y < 1000u) ||
      (dr16->data.ch_l_y > RC_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;             
	 dr16->data.res=  (fp32)((cbuf[16]|cbuf[17] << 8));
	return 1;
}
int8_t RC_ParseRC( DR16_t *dr16,LD_raw_t *LD_raw, LD_Data_t *LD, CMD_RC_t *rc) {
  if (dr16 == NULL) return DEVICE_ERR_NULL;
  if (LD == NULL) return DEVICE_ERR_NULL;
#ifdef DR16	
 	/*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)(RC_CH_VALUE_MAX - RC_CH_VALUE_MIN);

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

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

  rc->dr16.key = dr16->data.key;
  rc->dr16.res = ((float)dr16->data.res - RC_CH_VALUE_MID) ;
//  rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
	
	
#elif defined(LD_t)	
	
	/*乐迪*/
	rc->rc_type=RC_LD;
	LD_ParseRaw(LD_raw,LD);
	memcpy(&rc->LD, LD, sizeof(LD_Data_t)); 
	/*离线处理，和dr16位置不同*/

	if(LD_raw->sw[7] == 300||LD_raw->sw[3]!=1694)
	{
		LD_HandleOffline(LD,rc);
//    memset(cbuf, 0, sizeof(cbuf));	//有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信	
	}
#endif
  return DEVICE_OK;

}


/*dr16离线处理*/
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 ;

  (void)dr16;
  memset(&rc->dr16 , 0, sizeof(DR16_t));

  return 0;
}
int8_t LD_HandleOffline(const LD_Data_t *LD, CMD_RC_t *rc) {
	if (LD == NULL) return DEVICE_ERR_NULL;
  if (rc == NULL) return DEVICE_ERR_NULL;
  	rc->rc_type =Control_loss ;

	 memset(&rc->LD , 0, sizeof(LD_Data_t));
	return 0;
	
}



/*乐迪用的按键封装 */
CMD_SwitchPos_t MapSwitchState(int16_t value) {
  return (value > 300 && value < 500) ? CMD_SW_DOWN :
         (value >= 500 && value < 1500) ? CMD_SW_MID :
         (value >= 1500 && value < 1700) ? CMD_SW_UP : CMD_SW_ERR;
}