//Ãâ²¼Ïß»ùÕ¾¶Ë´úÂë
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
#include "dw_driver.h"
#include "Spi.h"
#include "global_param.h"
#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 uint16_t current_count,poll_startcount,start_count,end_count,poll_timeout;
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);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
//    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;
//        }
//    }        
	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;
}