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

#include <string.h>
#include "bsp/uart.h"
#include "device.h"

/* Private define ----------------------------------------------------------- */
#define DEAD_AREA 0.05f
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;

/* Private function --------------------------------------------------------- */
static void DEVICE_LIGHTBRIDGE2_RxCpltCallback(void)   //DMA空闲接收完成回调函数
{
    osThreadFlagsSet(thread_alert, SIGNAL_LIGHTBRIDGE2_RAW_REDY);
}

/* Exported functions ------------------------------------------------------- */
int8_t DEVICE_LIGHTBRIDGE2_Init(DEVICE_LIGHTBRIDGE2_t *lightbridge2)  //初始化
{
    if (lightbridge2 == NULL) return DEVICE_ERR_NULL;
    if (inited) return DEVICE_ERR_INITED;
    if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

    BSP_UART_RegisterCallback(BSP_UART_LIGHTBRIDGE2, BSP_UART_RX_CPLT_CB,
                              DEVICE_LIGHTBRIDGE2_RxCpltCallback);

    inited = true;
    return DEVICE_OK;
}

int8_t DEVICE_LIGHTBRIDGE2_Restart(void)   //重启LIGHTBRIDGE2串口
{
    __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_LIGHTBRIDGE2));
    __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_LIGHTBRIDGE2));
    return DEVICE_OK;
}

int8_t DEVICE_LIGHTBRIDGE2_StartDmaRecv(uint8_t *cmd_buffer)  //启动LIGHTBRIDGE2 DMA接收
{
    if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LIGHTBRIDGE2),
                             cmd_buffer,
                             DEVICE_LIGHTBRIDGE2_FRAME_LEN) == HAL_OK)
        return DEVICE_OK;
    return DEVICE_ERR;
}

bool DEVICE_LIGHTBRIDGE2_WaitDmaCplt(uint32_t timeout)   //等待LIGHTBRIDGE2 DMA接收完成
{
    return (osThreadFlagsWait(SIGNAL_LIGHTBRIDGE2_RAW_REDY,
                              osFlagsWaitAll, timeout) == SIGNAL_LIGHTBRIDGE2_RAW_REDY);
}
DEVICE_LIGHTBRIDGE2_Raw_t r;
//解析LIGHTBRIDGE2原始数据
void DEVICE_LIGHTBRIDGE2_ParseRaw(const uint8_t raw[DEVICE_LIGHTBRIDGE2_FRAME_LEN], DEVICE_LIGHTBRIDGE2_t *out)
{
//    DEVICE_LIGHTBRIDGE2_Raw_t r;

    //解析遥控器数据
    r.ch[0] = ((raw[1]      | raw[2]  << 8)                 & 0x07FF);
    r.ch[1] = ((raw[2] >> 3 | raw[3]  << 5)                 & 0x07FF);
    r.ch[2] = ((raw[3] >> 6 | raw[4] << 2 | raw[5] << 10)   & 0x07FF);
    r.ch[3] = ((raw[5] >> 1 | raw[6] << 7)                 & 0x07FF);

    //解析开关数据 
    r.sw[0] = ((raw[6] >> 4 | raw[7] << 4)                  & 0x07FF);
    r.sw[1] = ((raw[7] >> 7 | raw[8] << 1 | raw[9] << 9)    & 0x07FF);
    r.sw[2] = ((raw[9] >> 2 | raw[10]<< 6)                  & 0x07FF);
    r.sw[3] = ((raw[10]>> 5 | raw[11]<< 3)                  & 0x07FF);
    r.sw[4] = ((raw[12]     | raw[13]<< 8)                  & 0x07FF);
    r.sw[5] = ((raw[13]>> 3 | raw[14]<< 5)                  & 0x07FF);
    r.sw[6] = ((raw[14]>> 6 | raw[15]<< 2 | raw[16]<< 10)   & 0x07FF);
    r.sw[7] = ((raw[16]>> 1 | raw[17]<< 7)                  & 0x07FF);

    //映射遥控器数据
    #define MAP_SWITCH(v) \
        ((v) > 300 && (v) < 500) ? LIGHTBRIDGE2_CMD_SW_DOWN : \
        ((v) >= 500 && (v) < 1500) ? LIGHTBRIDGE2_CMD_SW_MID : \
        ((v) >= 1500 && (v) < 1700) ? LIGHTBRIDGE2_CMD_SW_UP : LIGHTBRIDGE2_CMD_SW_ERR
	
	#define MAP_SWITCH_2(v) \
        ((v) > 300 && (v) < 500) ? LIGHTBRIDGE2_CMD_SW_UP : \
        ((v) >= 500 && (v) < 1500) ? LIGHTBRIDGE2_CMD_SW_MID : \
        ((v) >= 1500 && (v) < 1700) ? LIGHTBRIDGE2_CMD_SW_DOWN : LIGHTBRIDGE2_CMD_SW_ERR
	
	out->data.ch_l_x = 2.0f*(r.ch[3]-209.0f)/(1596.0f-209.0f)-1.0f;
	out->data.ch_l_y = 2.0f*(r.ch[2]-178.0f)/(1575.0f-198.0f)-1.0f;
	out->data.ch_r_x = 2.0f*(r.ch[0]-210.0f)/(1596.0f-210.0f)-1.0f;
	out->data.ch_r_y = 2.0f*(r.ch[1]-221.0f)/(1604.0f-217.0f)-1.0f;
	
	if(fabs(out->data.ch_l_x)<=DEAD_AREA)out->data.ch_l_x = 0;
	if(fabs(out->data.ch_l_y)<=4*DEAD_AREA)out->data.ch_l_y = 0;
	if(fabs(out->data.ch_r_x)<=DEAD_AREA)out->data.ch_r_x = 0;
	if(fabs(out->data.ch_r_y)<=DEAD_AREA)out->data.ch_r_y = 0;
						
	if(fabs(out->data.ch_l_x)>=1.0f)out->data.ch_l_x= out->data.ch_l_x>0?1.0f:-1.0f;
	if(fabs(out->data.ch_l_y)>=1.0f)out->data.ch_l_y= out->data.ch_l_y>0?1.0f:-1.0f;
	if(fabs(out->data.ch_r_x)>=1.0f)out->data.ch_r_x= out->data.ch_r_x>0?1.0f:-1.0f;
	if(fabs(out->data.ch_r_y)>=1.0f)out->data.ch_r_y= out->data.ch_r_y>0?1.0f:-1.0f;  
//	out->data.ch_r_x 		= r.ch[0];
//	out->data.ch_r_y 		= r.ch[1];
//	out->data.ch_l_y		= r.ch[2];
//	out->data.ch_l_x 		= r.ch[3];
	
    out->data.key_A      = MAP_SWITCH_2(r.sw[0]);
    out->data.key_B      = MAP_SWITCH_2(r.sw[1]);
    out->data.key_C      = MAP_SWITCH_2(r.sw[2]);
    out->data.key_D      = MAP_SWITCH(r.sw[3]);
    out->data.key_E      = MAP_SWITCH(r.sw[4]);
    out->data.key_F      = MAP_SWITCH(r.sw[5]);
	out->data.key_G      = MAP_SWITCH_2(r.sw[6]);
	out->data.key_H      = MAP_SWITCH(r.sw[7]);
//    out->knob_left  = MAP_SWITCH(r.sw[6]);
//    out->knob_right = MAP_SWITCH(r.sw[7]);

    #undef MAP_SWITCH
	//check online.
	out->online = (r.sw[7] != 300&&r.ch[0] != 1000);
}