#include "dw_app.h"
int16_t target_offsettime = 850;
extern u32 synctimer;
extern u16 tagslotpos;
u32 target_time=100;
u8 pd_i;
u32 temptime,delaytime,expect_anctime;
int32_t error_time;
u16 delaycount;
extern u16 bigslot_num;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
extern LPTIM_HandleTypeDef hlptim1;
void NextPollDelay(u32 anchor_time)
{
    //tagslotpos = 0;
    error_time = anchor_time-expect_anctime;
    if(error_time<2000&&error_time>0)
        clockoffset += (target_offsettime-error_time)*0.1;
    for(pd_i=0; pd_i<tag_frequency; pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        {
            current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
            expect_anctime = temptime;
            return ;
        }
    }
    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
    expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
    return ;
}
u16 lpcount,poll_startcount,last_lpcount;
u16 waketopolltimeus = 3200;
#define BASENUM_COMTIME 244
u32 last_anchor_time;
static u16 current_count;
void SetLPTimer(u32 anchor_time,u8 nearbase_num)
{

    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}

void NextSlotDelayMs(int16_t delayms)
{
    current_slotnum++;
    if(current_slotnum>=tag_frequency)
    {
        current_slotnum = 0;
        lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
    } else {
        lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}
void WaitUntilPollStart(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;
        }
    }

}