#include "dw_mbx_anc.h"
///////////////////////////////////Ãâ²¼Ïßʱ¼äͬ²½´úÂë


typedef enum{	Sync_Lost, //ͬ²½¶ªÊ§         ͬ²½Ê±¼ä 100Ãëͬ²½Ò»´Î£¬Ã¿´Îͬ²½¿ªÆô2Ã룬·¢Æð²â¾àÇëÇó
                Sync_Range,//ͬ²½×´Ì¬ ²â¾à    ͬ²½Ê±¼ä 10Ãëͬ²½Ò»´Î£¬¿ªÆô10ms£¬·¢Æð²â¾àÇëÇó
                Sync_Config,//ͬ²½×´Ì¬ ÅäÖà   ͬ²½Ê±¼ä 1Ãëͬ²½Ò»´Î£¬²»·¢Æð²â¾àÇëÇó
                Sync_Sleep, //ͬ²½×´Ì¬ ÐÝÃß    ͬ²½Ê±¼ä 100Ãëͬ²½Ò»´Î£¬²»·¢Æð²â¾àÇëÇó
}sync_state_enum;
sync_state_enum sync_state = Sync_Lost;

static uint8_t sync_interval_s,range_state = 0;
static uint16_t sync_listen_ms;

void SyncStateSwitch(void) //ͬ²½Çл»Âß¼­£¬¿ª»úLOST״̬£¬ÊÕµ½Ðźźó½øÈëͬ²½×´Ì¬£¬ÔÚͬ²½×´Ì¬ÖУ¬¸ù¾ÝÊÕµ½µÄÍø¹ØÐÅÏ¢Åжϲâ¾à»¹ÊÇÐÝÃß ²â¾à ÅäÖÃ״̬
{
    switch(sync_state)
    {
        case Sync_Lost:
            sync_interval_s = 100;
            sync_listen_ms =2000;
            range_state = 1;
            break;
        case Sync_Range:
            sync_interval_s = 10;
            sync_listen_ms =5;
            range_state = 1;
            break;
        case Sync_Config:
            sync_interval_s = 1;
            sync_listen_ms =5;
            range_state = 0;
            break;
        case Sync_Sleep:
            sync_interval_s = 100;
            sync_listen_ms =5;
            range_state = 0;
            break;   
    }
}
uint8_t GetRangeState(void)
{
    return range_state;
}
#define SYNCLOST_THRES   10



static uint16_t current_count,poll_startcount,start_count,end_count,poll_timeout;
static uint8_t sync_interval_count = 0,flag_sync_success,sync_lost_count;
void SetFlagSyncSuccess(uint8_t value)
{
    flag_sync_success = value;
}

static uint16_t sync_wg_id,rec_sync_count;
uint8_t lora_recbuffer[255],lora_recbuffer_len;
static uint16_t synclock_zero_count,wg_sync_polltime_100us,uwbpoll_time_count;
uint16_t GetUwbPollCount(void)
{
    return uwbpoll_time_count;
}
uint16_t GetWGRecSyncCount(void)
{
    return rec_sync_count;
}
void SyncClockProcess(void) //ͬ²½Ê±ÖÓº¯Êý»ñÈ¡Ï´ÎUWBpollµÄ¶¨Ê±Æ÷count¡£
{
    memcpy(&wg_sync_polltime_100us,&lora_recbuffer[SYSTIME_IDX],2); //WGÏ·¢µÄͬ²½Ê±¼ä£¬µ¥Î»0.1ms¡£
    synclock_zero_count = rec_sync_count-(uint32_t)wg_sync_polltime_100us*100/LPTIMER_LSB;
    if(synclock_zero_count<0)
        synclock_zero_count+=32768;
    uwbpoll_time_count = synclock_zero_count + (uint32_t)g_com_map[UWBPOLLTIME_MS_IDX]*1000/LPTIMER_LSB; //É趨ºÃµÄUWB·¢ËÍʱ¼ä
    if(uwbpoll_time_count>32768)
        uwbpoll_time_count-=32768;
}
static uint16_t current_count,target_count;
void LoraSyncRecPoll(void) 
{ 
    if(sync_interval_count++>=sync_interval_s)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1); //Èç¹ûÐèÒª´¥·¢Í¬²½¾Í»áµÈ´ý´¥·¢Ê±¼ä£¬ÌáÇ°2ms´ò¿ª½ÓÊÕ
        target_count = GetWGRecSyncCount()-2000/LPTIMER_LSB; //ÌáÇ°2ms½øÈë½ÓÊÕ״̬
        if(target_count<0)
          target_count+=32768;
        while(current_count<target_count||current_count>target_count+16384)
        {
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
            if(current_count<target_count-300)
            {
                break;
            }
        }
        sync_interval_count = 0;
        flag_sync_success = 0;
        SwitchLoraSettings(SYNC_CHANNEL_FRQ,7); //Çл»lora½ÓÊÕƵµã//ÒòΪÊÇ´Ólorasleep»½Ðѵģ¬¿ÉÄÜÕâÀï»áÓõÄʱ¼ä¶à£¬ÐèÒª¿¼ÂÇ
        
        start_count=HAL_LPTIM_ReadCounter(&hlptim1);     //µÈ´ý½ÓÊÕÍê³ÉÖ±µ½³¬Ê±
        poll_timeout=sync_listen_ms*10; //µ¥Î»0.1ms
        end_count=start_count+(poll_timeout<<2);
        if(end_count>=32768)
        {end_count-=32768;}
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        SetFlagLoraWaitSync(1);
        Radio.Rx(100);
       while(GetFlagLoraWaitSync())
       {
         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         if(current_count>=end_count&&current_count<end_count+15000)
         {
                break;
         }
       }
       if(flag_sync_success)  //ͬ²½³É¹¦
       {
           sync_lost_count = 0;
           lora_recbuffer_len = GetLoraBufferAndLen(lora_recbuffer,&rec_sync_count);//»ñÈ¡LORAÊÕµ½µÄÊý¾ÝºÍ³¤¶È
           if(lora_recbuffer[MSG_TYPE_IDX]==LORA_MSGTYPE_SYNC)
           {
                memcpy(&g_com_map[SYNCWG_ID_IDX],&lora_recbuffer[WG_ID_IDX],2);
                if(lora_recbuffer[RANGE_STATE_IDX])
                {
                    sync_state = Sync_Range;
                }else{
                    sync_state = Sync_Sleep;
                }
                SyncStateSwitch();
                switch(lora_recbuffer[SUBMSGTYPE_IDX]) //×Ó״̬º¯Êý
                {
                    case SUBMSG_READ_ANCPARA:
                        
                        break;
                    case SUBMSG_WRITE_ANCPARA:
                        
                        break;
                    case SUBMSG_UPDATE_ANC:
                        if(!memcmp(&dev_id,&lora_recbuffer[ANCTAG_DEVICEID_IDX],2))
                        {LoraUp_Poll();}
                        break;
                }
           }
       }else{
            if(sync_lost_count++>SYNCLOST_THRES)
            {
                sync_state = Sync_Lost;
                SyncStateSwitch();
            }           
       }
   }
  //  Radio.Sleep( ); //Èç¹û³¬Ê±¾Ísleep
}
////////////////////////////Ãâ²¼Ïß²â¾àÏà¹Ø´úÂë


#define TAG_KEEPTIMES    30 //±êÇ©´æ»îʱ¼ä
#define MAX_TAG_LIST_NUM 200 //ͬʱͨѶ±êÇ©×î´óÊýÁ¿

static uint8_t uwb_sendbuffer[150];
static uint8_t rx_buffer[150];

uint16_t  taglist_total_num,taglist_current_index;  //µ±Ç°Áбí×ÜÊýÁ¿
static uint8_t  taglist_keeptime[MAX_TAG_LIST_NUM]; //ÿ¸ö±êÇ©´æ»îʱ¼ä
static uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //±êÇ©¾àÀëºÍ±êÇ©µÄID


static u8 frame_seq_nb,frame_len;

static uint32_t status_reg = 0;
extern int32_t dwt_readcarrierintegrator(void);
static int32_t test2;
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t get_tx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readtxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}

static uint64_t get_rx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readrxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}
//×¢Ò⣺ÒòΪÐÂ×¢²áµÄ±êÇ©²¢²»ÖªµÀ»ùÕ¾ÒѾ­ÊÕµ½ÁË£¬ËùÒÔÒªÔÚÏÂÒ»°üÁ¢¿Ì½«ÐµıêÇ©ID·¢³ö£¬·ñÔò±êÇ©»áÒ»Ö±ÇÀÕ¼»ùÕ¾µÄpoll°ü¡£
//·½·¨Êǽ«ÐÂ×¢²áµÄ±êÇ©Ìí¼Óµ½Ï´η¢Ë͵ıêÇ©ÁбíÍ·²¿£¬ÆäËû±êÇ©ÒÀ´ÎÏòºóŲ¶¯¡£
uint16_t AddNewTagIntoList(uint16_t tagid)  
{                                       
     u16 newindex_cal = taglist_current_index+25;
      if(newindex_cal>=taglist_total_num) //Èç¹ûµ±Ç°Ë÷ÒýÊÇ×îºóÒ»°ü£¬ÄÇô¾Í¼Óµ½×ÜË÷ÒýµÄºóÃ棬Èç¹û²»ÊǾͼӵ½ÏÂÒ»´ÎÆðʼË÷ÒýλÖá£
      {
        newindex_cal = taglist_total_num;
          
      }else{           
         for(u16 i=taglist_total_num;i>newindex_cal+1;i--)  //´ÓºóÍùǰŲµØ·½
            {
                taglist_id[i]=taglist_id[i-1];
                taglist_dist[i]=taglist_dist[i-1];
                taglist_keeptime[i]=taglist_keeptime[i-1];
            }          
      }
      taglist_total_num++;
      taglist_id[newindex_cal] = tagid;
      taglist_dist[newindex_cal] = 0;
      taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
      return newindex_cal;
}
uint16_t CmpTagInList(uint16_t tagid)
{
    u16 temp;
	for(u8 i=0;i<taglist_total_num;i++)
	{
		if(memcmp(&tagid,&taglist_id[i],2)==0)
        {
            taglist_keeptime[i] = TAG_KEEPTIMES;
			return i;
        }
	}
    if(taglist_total_num<MAX_TAG_LIST_NUM)
    temp = AddNewTagIntoList(tagid);
	return temp;
}
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
static int16_t anc_distoffset[MAX_NEARBASE_NUM];
static void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)  //дÈëʱ¼ä´ÁÐÅÏ¢
{
    memcpy(&anc_pollrx[i],pollrx,4);
    memcpy(&anc_resptx[i],resptx,4);
    memcpy(&tag_resprx[i],&resprx,4);
    memcpy(&anc_distoffset[i],distoffset,2);
    anc_clockoffset[i] = test2;
}
static float clockOffsetRatio;
#define FREQ_OFFSET_MULTIPLIER          (998.4e6/2.0/1024.0/131072.0)
#define FREQ_OFFSET_MULTIPLIER_110KB    (998.4e6/2.0/8192.0/131072.0)

#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1     (-1.0e6/3494.4e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2     (-1.0e6/3993.6e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3     (-1.0e6/4492.8e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5     (-1.0e6/6489.6e6)
static float clockOffsetRatio;
static double rtd_init, rtd_resp;
static double tof,distance;
#define SPEED_OF_LIGHT 299702547
u16 count1,count2;
static void CalculateDists(void)   //¼ÆËã¾àÀë
{
    for(int i=0;i<MAX_NEARBASE_NUM+10;i++) //ÒòΪÔö¼ÓÁË5¸öÇÀռλÖã¬ËùÒÔ×îÖյļÆËãÊÇ×ÜÊýÁ¿+10.¿¼ÂÇʵ¼ÊÇÀÕ¼¿ÉÄܳ¬¹ý5¸ö
    {
        //rec_anc_signalpower[i] = exsistbase_list[i];
      if(taglist_keeptime[i+taglist_current_index]==TAG_KEEPTIMES)
      {

         // exsistbase_list[i]--;
#ifdef _UWB_4G
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
#else
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
#endif
          rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff;
          rtd_resp = anc_resptx[i] - anc_pollrx[i];
          tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
          distance = tof * SPEED_OF_LIGHT;
          if(distance>-10&&distance<1000)
          {
              count2++;
            taglist_dist[i+taglist_current_index] = distance*100+anc_distoffset[i];
          }
      }else{
         // nearbase_distlist[i] = 0x1ffff;
      }
    
    }
}
void testprogram(void)
{
//    taglist_total_num = 30;
//    taglist_id[0] = 7;
//    for(uint8_t i=0;i<30;i++)
//    {
//        taglist_keeptime[i] = 100;
//    }
}
void TagListUpdate(void)
{
	uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
	for(i=0;i<taglist_total_num;i++)
	{
		if(taglist_keeptime[i]-->0)
		{
			taglist_id[j]=taglist_id[i];
			taglist_keeptime[j++]=taglist_keeptime[i];
		}
	}
	taglist_total_num=j;
}
static u16 rec_nearbaseid,temp_index;
static u8 rec_tag_index,tag_num_tosend;
void MBXANCPoll(void)  //½«Òª·¢µÄ±êÇ©ÐòºÅ£¬ÔÚÁбíÖеÄλÖÃ
{
    
    tag_num_tosend = taglist_total_num-taglist_current_index;
    if(tag_num_tosend>25)
    {
        tag_num_tosend = 25;
    }
    count1++;
    memcpy(&uwb_sendbuffer[TAG_ID_IDX],&dev_id,2);   
    uwb_sendbuffer[GROUP_ID_IDX] = group_id;
	uwb_sendbuffer[BATTARY_IDX] = bat_percent;
    uwb_sendbuffer[SEQUENCE_IDX] = frame_seq_nb++;
	uwb_sendbuffer[NEARBASENUM_INDEX] = tag_num_tosend;
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&taglist_id[taglist_current_index],tag_num_tosend*2);
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+tag_num_tosend*2],&taglist_dist[taglist_current_index],tag_num_tosend*2);
	uwb_sendbuffer[MESSAGE_TYPE_IDX] = MBX_POLL;
    dwt_writetxdata(13+4*tag_num_tosend, uwb_sendbuffer, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
	dwt_writetxfctrl(13+4*tag_num_tosend, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
    
	dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//¿ªÆô·¢ËÍ£¬·¢ËÍÍê³ÉºóµÈ´ýÒ»¶Îʱ¼ä¿ªÆô½ÓÊÕ£¬µÈ´ýʱ¼äÔÚdwt_setrxaftertxdelayÖÐÉèÖÃ	
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
    
    start_count=HAL_LPTIM_ReadCounter(&hlptim1);
    poll_timeout=tag_num_tosend*SLOT_SCALE+30; //µ¥Î»0.1ms
	end_count=start_count+(poll_timeout<<2);
	if(end_count>=32768)
	{end_count-=32768;}
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<end_count||current_count>end_count+15000)
        {
				current_count=HAL_LPTIM_ReadCounter(&hlptim1);
					while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
					{

							current_count=HAL_LPTIM_ReadCounter(&hlptim1);
						 if(current_count>=end_count&&current_count<end_count+15000)
								break;
					};
					if(status_reg==0xffffffff)
					{
						NVIC_SystemReset();
					}
					if (status_reg & SYS_STATUS_RXFCG)//Èç¹û³É¹¦½ÓÊÕ
						{
							dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//Çå³þ¼Ä´æÆ÷±ê־λ
							frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;	//»ñµÃ½ÓÊÕµ½µÄÊý¾Ý³¤¶È
							dwt_readrxdata(rx_buffer, frame_len, 0);   //¶ÁÈ¡½ÓÊÕÊý¾Ý
                            test2 = dwt_readcarrierintegrator();
                            poll_tx_ts = get_tx_timestamp_u64();										//»ñµÃPOLL·¢ËÍʱ¼äT1
                            resp_rx_ts = get_rx_timestamp_u64();
                            dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
							dwt_rxenable(0);//´ò¿ª½ÓÊÕ
							if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //ÅжϽÓÊÕµ½µÄÊý¾ÝÊÇ·ñÊÇresponseÊý¾Ý
							{	
								memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                                rec_tag_index = CmpTagInList(rec_nearbaseid);
                                temp_index =rec_tag_index -taglist_current_index;
                                SetANCTimestap(temp_index,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
                            }

                        }                      
                    }
    dwt_forcetrxoff();
    CalculateDists();            
    TagListUpdate();
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
    taglist_current_index+=tag_num_tosend;
    if(taglist_current_index>=taglist_total_num||taglist_total_num<=25) //Èç¹û±êÇ©µ±Ç°index´óÓÚ×Üindex£¬»òÕß×Ü»ùÕ¾ÊýÁ¿Ð¡ÓÚµÈÓÚ25£¬Ôò´Ó0¿ªÊ¼index¡£
        taglist_current_index = 0;
}