#include "remote_control.h"
#include "main.h"

extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;
#if FREERTOS == 0
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
static int map(int x, int in_min, int in_max, int out_min, int out_max);

RC_ctrl_t rc_ctrl;
static uint8_t sbus_rx_buf[2][RC_FRAME_LENGTH];
//串口dma双缓冲区初始化

#if FREERTOS == 0
//串口中断
void USART3_IRQHandler(void)
{
	//have received data 
	if(huart3.Instance->SR & UART_FLAG_RXNE)
	{
		//如果是接收中断则通过读取dr寄存器清零
		__HAL_UART_CLEAR_FEFLAG(&huart3);
	}
	if(USART3->SR & UART_FLAG_IDLE)
	{
		//使用清除pe标志位的函数是因为pe idle等几个中断都是靠先读取sr再读取dr清零的
    static uint16_t this_time_rx_len = 0;
		__HAL_UART_CLEAR_PEFLAG(&huart3);
		
		if( (hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
		{
			//current memory buffer used is memory0
			
			//disable dma to change dma register
			__HAL_DMA_DISABLE(&hdma_usart3_rx);
			
			//get received data length, length = set_data_length - remain_length 
			this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;

			//reset set_data_length
			hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
			
			//change memory0 to memory1
			hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
			
			//enable dma
			__HAL_DMA_ENABLE(&hdma_usart3_rx);
			
			//1 frame length is correct data
			if(this_time_rx_len == RC_FRAME_LENGTH)
			{
				sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
			}
		}
			else
			{
				__HAL_DMA_DISABLE(&hdma_usart3_rx);
				
				this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
				
				hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
				
				//change memory1 to memory0
				DMA1_Stream1->CR &= ~(DMA_SxCR_CT);
				
				__HAL_DMA_ENABLE(&hdma_usart3_rx);
				
				if(this_time_rx_len == RC_FRAME_LENGTH)
				{
					sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
				}
			}
	}
}
#else
//如果打开了freertos，则只进行任务通知
void USART3_IRQHandler(void)
{

}
#endif

void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);

void remote_control_init(void)
{
//	BSP_UART_RegisterCallback(BSP_UART_DR16,BSP_UART_IDLE_LINE_CB,DR16_IDLE_CB);//还没有移入嘉宁的架构，待测试
	RC_init(sbus_rx_buf[0], sbus_rx_buf[1], RC_FRAME_LENGTH);
	
}


static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
{
    if (sbus_buf == NULL || rc_ctrl == NULL)
    {
        return;
    }
		
	rc_ctrl->ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //Channel 1
    rc_ctrl->ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //Channel 2
    rc_ctrl->ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //Channel 3
                         (sbus_buf[5] << 10)) &0x07ff;
    rc_ctrl->ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //Channel 4
    
	rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                   								//Channel 5
    rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;    	//Channel 6
    rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    							//Channel 7
    rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    							//Channel 8
    rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  								//Channel 9
    rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                               	//Channel 10
    rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;	//Channel 11
    rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    							//Channel 12

	
    rc_ctrl->sw[2] = map(rc_ctrl->sw[2],306,1694,1694,306);	
    rc_ctrl->sw[3] = map(rc_ctrl->sw[3],306,1694,1694,306);	
	rc_ctrl->ch[1] = map(rc_ctrl->ch[1],-693,694,-700,700);		//x
	rc_ctrl->ch[0] = map(rc_ctrl->ch[0],-694,693,-700,700);		//y

	//死区(-30,30)
	if(rc_ctrl->ch[0]>-14&&rc_ctrl->ch[0]<6) rc_ctrl->ch[0]=0;              
	if(rc_ctrl->ch[1]>-30&&rc_ctrl->ch[1]<-10) rc_ctrl->ch[1]=0;                
	if(rc_ctrl->ch[2]>=0&&rc_ctrl->ch[2]<=7) rc_ctrl->ch[2]=7;              
	if(rc_ctrl->ch[3]>-22&&rc_ctrl->ch[3]<-2) rc_ctrl->ch[3]=0;        

}                                                                            																																					



int map(int x, int in_min, int in_max, int out_min, int out_max)		//映射函数
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


/*



  																																	
306																							  					  	  306
 sw[]																						 	 					  		sw[7]
1694																							 					  		1694
 
306																														    	306
sw[6]                                                              sw[4]
1694                                                               1694

306         306                                        306        306   
sw[0]      sw[2]    sw[1]:306-1694   sw[5]:306-1694  sw[]1000   sw[3] 
1694       1694                                         1694       1694
                                      710
               688                   1425
                |                      |
                |                      |
   54    -616------ch[3]770   -354---------ch[0] 339  0
                |                      |
                |                      |
	             ch[2]                 ch[1]
               _699                   38   
*/


void RC_init(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)
{
	
	//enable the dma transfer for the receiver request
	SET_BIT(huart3.Instance->CR3, USART_CR3_DMAR);
	
	//enable idle interrupt
	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
	
	//disable dma, to change the dma register
	__HAL_DMA_DISABLE(&hdma_usart3_rx);
	
	while(hdma_usart3_rx.Instance->CR & DMA_SxCR_EN)
	{
		__HAL_DMA_DISABLE(&hdma_usart3_rx);
	}
	
	hdma_usart3_rx.Instance->PAR = (uint32_t) & (USART3->DR);
	
	//memory buffer 1
	hdma_usart3_rx.Instance->M0AR = (uint32_t)(rx1_buf);
	
	//momory buffer 2
	hdma_usart3_rx.Instance->M1AR = (uint32_t)(rx2_buf);
	
	//data length
	hdma_usart3_rx.Instance->NDTR = dma_buf_num;
	
	//enable double memory buffer
	SET_BIT(hdma_usart3_rx.Instance->CR, DMA_SxCR_DBM);
	
	
	//enable dma
	__HAL_DMA_ENABLE(&hdma_usart3_rx);

}

#else
//DMA双缓冲区+串口空闲中断

// 02 53 44 1x1 3x2 

static osEventFlagsId_t eventReceive;

RC_mess_t RC_mess;

#define hdma_rc hdma_usart3_rx
#define uart_rc huart3
#define per_rc  USART3
/* Private function  -------------------------------------------------------- */
static void RC_RxCpltCallback(void) {
	osEventFlagsSet(eventReceive, EVENT_RC);

	if((hdma_rc.Instance->CR & DMA_SxCR_CT) == 0u)//DMA_SxM0AR
	    	RC_mess.dma_buffer = DMA_SxM0AR;
	else 	RC_mess.dma_buffer = DMA_SxM1AR;			
}

static int map(int x, int in_min, int in_max, int out_min, int out_max)		//映射函数
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
#if DEBUG == 0
static RC_data_t rc_buff;
#else
RC_data_t rc_buff;
#endif
//sbus协议解析
static int8_t sbus_to_rc( uint8_t *sbus_buf, RC_data_t *rc_ctrl)
{
    if (sbus_buf == NULL || rc_ctrl == NULL)
        return DEVICE_ERR_NULL;
    
		rc_buff.ch[0] = (sbus_buf[1] | (sbus_buf[2] << 8)) & 0x07ff;        //!< Channel 0
		rc_buff.ch[1] = ((sbus_buf[2] >> 3) | (sbus_buf[3] << 5)) & 0x07ff; //!< Channel 1
		rc_buff.ch[2] = ((sbus_buf[3] >> 6) | (sbus_buf[4] << 2) |          //!< Channel 2
												 (sbus_buf[5] << 10)) &0x07ff;
		rc_buff.ch[3] = ((sbus_buf[5] >> 1) | (sbus_buf[6] << 7)) & 0x07ff; //!< Channel 3
			
		rc_ctrl->sw[0] = ((int16_t)sbus_buf[6] >> 4 | ((int16_t)sbus_buf[7] << 4 )) & 0x07FF;                  //!< Switch left
		rc_ctrl->sw[1] = ((int16_t)sbus_buf[7] >> 7 | ((int16_t)sbus_buf[8] << 1 ) | (int16_t)sbus_buf[9] << 9 ) & 0x07FF;                       //!< Switch right
		rc_ctrl->sw[2] = ((int16_t)sbus_buf[9] >> 2 | ((int16_t)sbus_buf[10] << 6 )) & 0x07FF;;                    //!< Mouse X axis
		rc_ctrl->sw[3] = ((int16_t)sbus_buf[10] >> 5 | ((int16_t)sbus_buf[11] << 3 )) & 0x07FF;                    //!< Mouse Y axis
		rc_ctrl->sw[4] = ((int16_t)sbus_buf[12] << 0 | ((int16_t)sbus_buf[13] << 8 )) & 0x07FF;                  //!< Mouse Z axis
		rc_ctrl->sw[5] = ((int16_t)sbus_buf[13] >> 3 | ((int16_t)sbus_buf[14] << 5 )) & 0x07FF;                                  //!< Mouse Left Is Press ?
		rc_ctrl->sw[6] = ((int16_t)sbus_buf[14] >> 6 | ((int16_t)sbus_buf[15] << 2 ) | (int16_t)sbus_buf[16] << 10 ) & 0x07FF;                                  //!< Mouse Right Is Press ?
		rc_ctrl->sw[7] = ((int16_t)sbus_buf[16] >> 1 | ((int16_t)sbus_buf[17] << 7 )) & 0x07FF;                    //!< KeyBoard value

   
		rc_buff.ch[0] = map(rc_buff.ch[0],1693,306,-800,800);//x
		rc_buff.ch[1] = map(rc_buff.ch[1],1659,272,-800,800);//y
		rc_buff.ch[2] = map(rc_buff.ch[2],1693,337,0,100);//mul
		rc_buff.ch[3] = map(rc_buff.ch[3],1690,314,-800,800);//w

	
		if(rc_buff.ch[0]>-30&&rc_buff.ch[0]<30) rc_buff.ch[0]=0;
		if(rc_buff.ch[1]>-30&&rc_buff.ch[1]<30) rc_buff.ch[1]=0;
		if(rc_buff.ch[2]>-30&&rc_buff.ch[2]<30) rc_buff.ch[2]=0;
		if(rc_buff.ch[3]>-30&&rc_buff.ch[3]<30) rc_buff.ch[3]=0;
		
		rc_ctrl->ch[0] = rc_buff.ch[0];
		rc_ctrl->ch[1] = rc_buff.ch[1];
		rc_ctrl->ch[2] = rc_buff.ch[2];
		rc_ctrl->ch[3] = rc_buff.ch[3];
		
		return DEVICE_OK;
}

static void RC_restart(int8_t *Error_count)
{
    __HAL_UART_DISABLE(&uart_rc);
    __HAL_DMA_DISABLE(&hdma_rc);

    hdma_rc.Instance->NDTR = 25u;

    __HAL_DMA_ENABLE(&hdma_rc);
    __HAL_UART_ENABLE(&uart_rc);
		*Error_count = 0;
}


/* Exported functions ------------------------------------------------------- */
int8_t RC_init()
{
	uart_rc.Instance->CR3 |= USART_CR3_DMAR; 		 //接收使能DMA模式
	__HAL_UART_ENABLE_IT(&uart_rc, UART_IT_IDLE); //串口空闲中断使能
//	uart_rc.Instance->CR1 |= USART_CR1_IDLEIE;//中断使能
//	DMA1_Stream1->CR |= (1<<4);   				 //DMA传输完成中断
	
	while(hdma_rc.Instance->CR & DMA_SxCR_EN)
    {
		__HAL_DMA_DISABLE(&hdma_rc);
	}
	hdma_rc.Instance->PAR = (uint32_t)&(per_rc->DR);
	hdma_rc.Instance->M0AR= (uint32_t)&RC_mess.sbus_rx_buffer[0][0];
	hdma_rc.Instance->M1AR= (uint32_t)&RC_mess.sbus_rx_buffer[1][0];
	hdma_rc.Instance->NDTR= 25u;
	hdma_rc.Instance->CR |= DMA_SxCR_DBM;//使用双缓冲区
	hdma_rc.Instance->CR &= ~DMA_SxCR_CT;//使用DMA_SxMOAR指针寻址

	__HAL_DMA_ENABLE(&hdma_rc);
	
	BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_IDLE_LINE_CB,
                            RC_RxCpltCallback);

	return DEVICE_OK;
}

uint32_t RC_WaitNew() {
  return osEventFlagsWait(
				eventReceive, EVENT_RC,osFlagsWaitAny, osWaitForever);
}

int8_t RC_rx_analysis(){

	switch(RC_mess.dma_buffer)
	{
	case DMA_SxM0AR:
		if(RC_mess.sbus_rx_buffer[0][0] == 0x0F){
			sbus_to_rc(RC_mess.sbus_rx_buffer[1], &RC_mess.RC_data);
			RC_mess.Error_flag = DATA_OK;
		}else{
			RC_mess.Error_flag = DATA_ERROR;
		}
		
		break;
		
	case DMA_SxM1AR:
		if(RC_mess.sbus_rx_buffer[1][0] == 0x0F){
			sbus_to_rc(RC_mess.sbus_rx_buffer[0], &RC_mess.RC_data);
			RC_mess.Error_flag = DATA_OK; 
		}else{
			RC_mess.Error_flag = DATA_ERROR;
		}
			break;
	}
	
	//累计三次数据错误，重启
	RC_mess.Error_flag == DATA_OK ? RC_mess.Error_count-- : RC_mess.Error_count++;
	if(RC_mess.Error_count>3)	RC_restart(&RC_mess.Error_count);
	if(RC_mess.Error_count<0)	RC_mess.Error_count=0;
	RC_mess.Error_flag = 0;
	
	return DEVICE_OK;
}

RC_data_t* get_rc_data()
{
    return &RC_mess.RC_data;
}


#endif
