#include "dw_tag.h"
#include "stdint.h"
#include <string.h>
#include "mk_uwb.h"
#include "mk_misc.h"
#include "mk_uart.h"
#include "mk_trace.h"
#include "global_param.h"
static float clockOffsetRatio;
static double rtd_init, rtd_resp;
double tof,distance_tag;
#define SPEED_OF_LIGHT 299702547
#define DWT_TIME_UNITS          (1.0/499.2e6/128.0) //!< = 15.65e-12 s
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM],tag_polltx[MAX_NEARBASE_NUM];
static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
static int16_t anc_distoffset[MAX_NEARBASE_NUM];
extern uint8_t  taglist_keeptime[MAX_TAG_LIST_NUM]; //ÿ¸ö±êÇ©´æ»îʱ¼ä
extern uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //±êÇ©¾àÀëºÍ±êÇ©µÄID
extern uint16_t  taglist_total_num,taglist_current_index;
//#if defined (MK_SS_TWR_DW_INIT)
extern uint8_t exsistbase_list[MAX_NEARBASE_NUM];
extern int32_t nearbase_distlist[MAX_NEARBASE_NUM];
static int64_t tround,treply;
static int64_t poll_tx_timestamp,resp_rx_timestamp,resp_tx_timestamp,poll_rx_timestamp;

int32_t temp_freq_offset;
int64_t tround_temp,treply_temp;
int32_t tof_i;
uint32_t tof_i_ui;
double tof_f;
extern int32_t freq_offset,freq_offset_filter; 
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset,int32_t anc_clockoffset_from_MK,uint32_t polltx)  //дÈëʱ¼ä´ÁÐÅÏ¢
{
    memcpy(&anc_pollrx[i],pollrx,4);
    memcpy(&anc_resptx[i],resptx,4);
    memcpy(&tag_resprx[i],&resprx,4);
    memcpy(&anc_distoffset[i],distoffset,2);
		memcpy(&tag_polltx[i],&polltx,4);
    //anc_clockoffset[i] = anc_clockoffset_from_MK;//MK8000ÐÞ¸Ä
}
//void CalculateDists1(int64_t poll_tx_ts) 
//{
//for(int i=0;i<MAX_NEARBASE_NUM;i++)
//    {
//        //rec_anc_signalpower[i] = exsistbase_list[i];
//      if(exsistbase_list[i]==KEEP_TIMES)
//      {
//         // 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) ;//MK8000ÐÞ¸Ä
//#endif
//          rtd_init = tag_resprx[i] - (uint32_t)poll_tx_ts;
//          rtd_resp = anc_resptx[i] - anc_pollrx[i];
//          tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
//          distance_tag = tof * SPEED_OF_LIGHT/100;
//          if(distance_tag>-10&&distance_tag<1000)
//          {
//            nearbase_distlist[i]  = distance_tag+anc_distoffset[i];
//          }
//      }else{
//          nearbase_distlist[i] = 0x1ffff;
//      }
//    }
//}
// ts_a - ts_b
int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b)
{
    if (ts_a < ts_b)
    {
        return (0x20000000000 - ts_b + ts_a);
    }
    else
    {
        return (ts_a - ts_b);
    }
}