#include "wirless_uwb_app.h"

#define MAXSEARCHSYNCTIMES  300
E_RangProc rangproc;
static u16 current_count,start_count,end_count,poll_startcount;
static u16 lpcount,last_lpcount;
static u16 waketopolltimeus = 3200;
#define BASENUM_COMTIME 244
static u32 last_anchor_time;
static int32_t error_time;
static u32 temptime,delaytime,expect_anctime;
static int16_t poll_offsettime=-6900,clockoffset1S;
int16_t target_offsettime = 850;
static u16 searchsync_times;
static u16 stime_time,syncbase_ms,range_ms,stime_lp_difftime;//ͬ²½ÏµÍ³Ê±¼äºÁÃë ΢Ãͬ²½»ùվʱ¼ä£¬²â¾àʱ¼ä¡£
void CalibClockOffset(int16_t clockerror)
{
    if(clockerror<2000&&error_time>-2000)
    clockoffset1S+=clockerror*0.1;
}
void SetWkupTimeBaseNow(u16 current_lpcount,u32 delaytime)  //Ï´λ½ÐÑʱ¼äºÍµ±Ç°Ê±¼äµÄ²îÖµ¡£
{
		//tagslotpos = 0;

	//CalibClockOffset(error_time);

    delaytime += poll_offsettime+clockoffset1S*delaytime/1000000;
    lpcount = current_lpcount+delaytime/LPTIMER_LSB;
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }		
    poll_startcount = lpcount+105;//105=(3200)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
        last_lpcount = lpcount;
    
   return ;
}

void SetWkupTimeBaseLast(u32 delaytime)  //ÏÂÒ»´ÎµÄ»½ÐÑʱ¼ä±ÈÉÏÒ»´ÎµÄ»½ÐÑʱ¼ä²î¶àÉÙ£¬Èç¹ûÊÇ0»òÕßÊÇ1000msÔò²»Óõ÷Óá£
{
    lpcount = last_lpcount+delaytime/LPTIMER_LSB;
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }		
    poll_startcount = lpcount+105;//105=(3200)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
        last_lpcount = lpcount;
}
void WkupUWB(void)
{   u32 id;
    u8 error_times;
    SPIx_CS_GPIO->BRR = SPIx_CS;
	delay_us(700);
	SPIx_CS_GPIO->BSRR = SPIx_CS;
	id =  dwt_readdevid() ;
	    while (DWT_DEVICE_ID != id) 
    {
			id =  dwt_readdevid() ;
         IdleTask();	
        if(error_times++>20)
        {
            printf("DW ID ERROR.\r\n");
            SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //Èí¸´Î»»Øµ½bootloader   
        }
    }
}
void WaitUntilTargetTime(void)
{

		current_count=HAL_LPTIM_ReadCounter(&hlptim1);
		
		while(current_count<poll_startcount||current_count>poll_startcount+16384)
		{
			current_count=HAL_LPTIM_ReadCounter(&hlptim1);
			if(current_count<poll_startcount-300)
			{
				break;
			}
		}
}
static u32 status_reg;
static u8 frame_len;
static u8 rx_buffer[130];
void WaitConfigProc(void) //¿ªÆô½ÓÊÕ1ms£¬ÐÞ¸ÄÅäÖá£
{
    WkupUWB();
   // WaitUntilTargetTime();
    dwt_setrxtimeout(1000);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä10
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
    {}
    if(status_reg & SYS_STATUS_RXFCG)
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
		frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È      
		dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
    }else{
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }                                             
}
void SearchSyncProc(void)
{
    WkupUWB();
    WaitUntilTargetTime();
    dwt_setrxtimeout(10000);//½ÓÊÕ10msÊý¾Ý
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
    {}
    if(status_reg & SYS_STATUS_RXFCG)
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
		frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È      
		dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
    }else{
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }  
    SetWkupTimeBaseLast(980);  
    searchsync_times++;    
    if(searchsync_times>MAXSEARCHSYNCTIMES)
    {
        
    }
}

void WaitSyncProc(void) //Õý³£Í¬²½
{
    WkupUWB();
    WaitUntilTargetTime();
    dwt_setrxtimeout(1000);//½ÓÊÕ10msÊý¾Ý
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
    {}
    if(status_reg & SYS_STATUS_RXFCG)
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//Çå³ý±ê־λ
		frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È      
		dwt_readrxdata(rx_buffer, frame_len, 0);//¶ÁÈ¡½ÓÊÕÊý¾Ý
    }else{
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }  
}
u32 DelayBetweenTimes(u32 target_time,u32 current_time)
{
    int32_t temp_time;
    temp_time = target_time-current_time;
    while(temp_time>1000000)
    {
        temp_time -= 1000000;
    }
    while(temp_time<=0)
    {
        temp_time += 1000000;
    }
        
    return temp_time;
}
void NealPoll(void)
{

}
void RangingProc(void)
{
    u32 current_lptime;
    current_lptime = HAL_LPTIM_ReadCounter(&hlptim1)*LPTIMER_LSB; //µÍ¹¦ºÄʱÖÓ»»Ëãʱ¼ä
    stime_time = current_lptime + stime_lp_difftime;              //ͬ²½Ê±¼ä
    if(DelayBetweenTimes(syncbase_ms*1000,stime_time)>DelayBetweenTimes(range_ms*1000,stime_time))
    {
        rangproc = RANGPROC_NEARPOLL;
    }else{
        rangproc = RANGPROC_WAITSYNC;
    }
}