/*! ----------------------------------------------------------------------------
 *  @file    instance_common.c
 *  @brief   DecaWave application level common instance functions
 *
 * @attention
 *
 * Copyright 2015 (c) DecaWave Ltd, Dublin, Ireland.
 *
 * All rights reserved.
 *
 * @author DecaWave
 */
#include "bsp.h"
#include "compiler.h"
#include "port.h"
#include "deca_device_api.h"
#include "deca_spi.h"

#include "instance.h"


// -------------------------------------------------------------------------------------------------------------------
//      Data Definitions
// -------------------------------------------------------------------------------------------------------------------
#define DW_MAX_POWER            1

// -------------------------------------------------------------------------------------------------------------------

extern char debugBuffer[256];
extern int debugLen;

volatile instance_data_t instance_data[NUM_INST] ;
volatile instance_data_t *gInst = &instance_data[0];


volatile uint32_t ancRespCount=0;
volatile stAncRespRecord ancRespRecords[8] = {0};

volatile stTagFinalRecord tagFinalRecord = {0};

volatile stInstDebug instDebug={0};

volatile eRxTxStage trxStage = RTX_IDLE;
// -------------------------------------------------------------------------------------------------------------------
// Functions
// -------------------------------------------------------------------------------------------------------------------

extern void DebugRun(void);
extern int SendMessageWifi(uint8_t* buf, int len);
extern int SendMessage(uint8_t* buf,int len);
extern int SendMessageDbg(uint8_t* buf,int len);

void ancPrepareResponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp);

void instClearDistTableAll(void);
uint8_t tagRXReEnable(uint16_t sourceAddress);
uint8_t ancSendFinalorRXReEnable(uint16_t sourceAddress);
void ancEnableRX(void);
uint8_t ancTXorRXReEnable(uint16_t sourceAddress);
void handle_error_unknownFrame(event_data_t dw_event);

float calculatePower(float base, float N, uint8_t pulseFrequency) 
{
    float A, corrFac;

    if(DWT_PRF_16M == pulseFrequency) {
        A = 115.72;
        corrFac = 2.3334;
    } else {
        A = 121.74;
        corrFac = 1.1667;
    }

    float estFpPwr = 10.0 * log10(base / (N * N)) - A;

    if(estFpPwr <= -88) {
        return estFpPwr;
    } else {
        // approximation of Fig. 22 in user manual for dbm correction
        estFpPwr += (estFpPwr + 88) * corrFac;
    }

    return estFpPwr;
}

float dwGetReceivePower(void) 
{
    uint32_t D17F=pow(2,17);
    dwt_rxdiag_t rxdiag_t;
    rxdiag_t.maxGrowthCIR=0;
	rxdiag_t.rxPreamCount=0;
    dwt_readdignostics(&rxdiag_t);
    float C = (&(rxdiag_t.stdNoise))[3];
    float N = rxdiag_t.rxPreamCount;

    int RX_level_C=(int)rxdiag_t.maxGrowthCIR;
	int RX_level_N=(int)rxdiag_t.rxPreamCount;

    float RX_level_A = 0;
    if(gInst->setting.configData.prf== DWT_PRF_64M)
            RX_level_A = 121.74;
    if(gInst->setting.configData.prf== DWT_PRF_16M)
            RX_level_A = 113.77;

    float RX_level = 0;
    RX_level=RX_level_C*D17F;
    RX_level=RX_level/(RX_level_N*RX_level_N);
    RX_level=10*log10(RX_level);
    RX_level=RX_level-RX_level_A;
    return RX_level;
}

void PrintDebugMessage(void)
{
    debugLen = 0;
    debugLen += sprintf(debugBuffer+debugLen,"Debug: p:%06d, a: %06d, %06d %06d %06d %06d, f:%06d s:%06d ",
                      instDebug.ancPollCount, instDebug.andRespSentCount,
                      instDebug.ancRespCounts[0],instDebug.ancRespCounts[1],
                      instDebug.ancRespCounts[2],instDebug.ancRespCounts[3],
                      instDebug.ancFinalCount,instDebug.tofSuccess);

    debugLen += sprintf(debugBuffer+debugLen,"arf: %06d %06d %06d %06d, asf: %6d, rt:%06d, calc:%d errFrm:%d T:%d\r\n",
                      instDebug.ancRespFailsCounts[0],instDebug.ancRespFailsCounts[1],
                      instDebug.ancRespFailsCounts[2],instDebug.ancRespFailsCounts[3],
                      instDebug.ancRespLateCount,instDebug.rxTimeoutCount,
                      instDebug.calculateFails,
                      instDebug.errFrameCount,
                      portGetTickCnt());
#if defined(REPORT2COM)
    ReportMessage((uint8_t*)debugBuffer, debugLen);
#elif defined(REPORT2WIFI)
    SendMessageWifi((uint8_t*)debugBuffer, debugLen);
#endif
    SendMessage((uint8_t*)debugBuffer,debugLen);
    SendMessageDbg((uint8_t*)debugBuffer,debugLen);
}

// -------------------------------------------------------------------------------------------------------------------
// convert microseconds to device time
uint64_t convertMicroSec2DeviceTime (double microSec)
{
    uint64_t dt;
    long double dTime;
    dTime = (microSec / (double) DWT_TIME_UNITS) / 1e6 ;
    dt =  (uint64_t) (dTime) ;
    return dt;
}


double convertDeviceTime2Sec(int32_t dt)
{
    double f = 0;
    f =  dt * DWT_TIME_UNITS ;  // seconds #define TIME_UNITS          (1.0/499.2e6/128.0) = 15.65e-12
    return f ;
}



#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
int reportTOF(int idx, stTofInformation* tofx)
{
    double distance ;
    double distance_to_correct;
    double tof ;
    int32_t tofi ;

    // check for negative results and accept them making them proper negative integers
    tofi = (int32_t) tofx->value ; // make it signed
    if (tofi > 0x7FFFFFFF)  // close up TOF may be negative
    {
        tofi -= 0x80000000 ;  //
    }

    // convert to seconds (as floating point)
    tof = convertDeviceTime2Sec(tofi) ;          //this is divided by 4 to get single time of flight
    //inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;
    distance = tof * SPEED_OF_LIGHT;
    gInst->twrResult.distance[idx].distanceRaw = distance;
    gInst->twrResult.distance[idx].address = tofx->address;

#if (CORRECT_RANGE_BIAS == 1)
    //for the 6.81Mb data rate we assume gating gain of 6dB is used,
    //thus a different range bias needs to be applied
    //if(inst->configData.dataRate == DWT_BR_6M8)
    if(gInst->setting.configData.smartPowerEn)
    {
        //1.31 for channel 2 and 1.51 for channel 5
        if(gInst->setting.configData.chan == 5)
        {
            distance_to_correct = distance/1.51;
        }
        else //channel 2
        {
            distance_to_correct = distance/1.31;
        }
    }
    else
    {
        distance_to_correct = distance;
    }

    distance = distance - dwt_getrangebias(gInst->setting.configData.chan, (float) distance_to_correct, gInst->setting.configData.prf);
#endif

    if ((distance < 0) || (distance > 20000.000))    // discard any results less than <0 cm or >20 km
    {
        return 0;
    }

    gInst->twrResult.distance[idx].distanceBias = distance;

    gInst->tmAndDelays.longTermRangeCount++ ;                          // for computing a long term average

    return 1;
}

/*
void setTagDist(int tidx, int aidx)
{
	//inst_tdist[tidx] = inst_idist[aidx];
    inst_tagdist[tidx].distanceBias = inst_dist[aidx].distanceBias;
    inst_tagdist[tidx].distanceRaw = inst_dist[aidx].distanceRaw;
}

double getTagDist(int idx)
{
	//return inst_tdist[idx];
    return inst_tagdist[idx].distanceBias;
}*/


void clearDistTable(int idx)
{
    gInst->twrResult.distance[idx].address = BROADCAST_ADDR;
    gInst->twrResult.distance[idx].distanceBias = 0;
    gInst->twrResult.distance[idx].distanceRaw = 0;
}



void instClearDistTableAll(void)
{
    int i;
    for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)
    {
        clearDistTable(i);
    }
}

int instNewRange(void)
{
    int x = gInst->stateMachine.newRange;
    gInst->stateMachine.newRange = TOF_REPORT_NUL;
    return x;
}

int instNewRangeAncAdd(void)
{
    return gInst->stateMachine.newRangeAncAddress;
}

int instNewRangeTagAdd(void)
{
    return gInst->stateMachine.newRangeTagAddress;
}

uint32_t instNewRangeTim(void)
{
    return gInst->stateMachine.newRangeTime;
}

// -------------------------------------------------------------------------------------------------------------------
// function to clear counts/averages/range values
//
void ClearRangeInformation(int index)
{
    int j;
    memset((void*)(gInst->rangeInform + index), 0, sizeof(stTagRangeInformation));
    gInst->rangeInform[index].address = BROADCAST_ADDR;
    gInst->rangeInform[index].tofOfTags = INVALID_TOF;

    /*for(j=0;j<RESP_RECORD_COUNT;j++)
    {
        gInst->rangeInform[index].rxResps[j] = -10;
    }*/
    memset((void*)gInst->rangeInform[index].rxResps, -10, RESP_RECORD_COUNT);
}


void instClearCounts(void)
{
    int i= 0;

    dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP

    gInst->msgFrame.frameSN = 0;
    gInst->tmAndDelays.longTermRangeCount  = 0;

    for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)
    {
        gInst->twrResult.tofArray[i].address = BROADCAST_ADDR;
        gInst->twrResult.tofArray[i].value = INVALID_TOF;
    }

    for(i=0; i<MAX_TAG_LIST_SIZE; i++)
    {
        ClearRangeInformation(i);
    }
} // end instanceclearcounts()


// -------------------------------------------------------------------------------------------------------------------
// function to initialise instance structures
//
// Returns 0 on success and -1 on error
int instance_init(void)
{
    int result;
    
    // assume listener,
    gInst->stateMachine.testAppState = TA_INIT ;

    // Reset the IC (might be needed if not getting here from POWER ON)
    // ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file
    //dwt_softreset();

	//this initialises DW1000 and uses specified configurations from OTP/ROM
    result = dwt_initialise(DWT_LOADUCODE | DWT_LOADLDOTUNE | DWT_LOADTXCONFIG | DWT_LOADANTDLY| DWT_LOADXTALTRIM) ;

    //this is platform dependent - only program if DW EVK/EVB
    dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs

    if (DWT_SUCCESS != result)
    {
        return (-1) ;   // device initialise has failed
    }


    instClearCounts() ;

    gInst->stateMachine.wait4ack = 0;

    instClearEvents();

    //dwt_geteui(instance_data[instance].eui64);
    memset((void*)gInst->setting.eui64, 0, ADDR_BYTE_SIZE_L);

    gInst->tmAndDelays.tagSleepCorrection = 0;

    dwt_setautorxreenable(0); //disable auto RX re-enable
    dwt_setdblrxbuffmode(0); //disable double RX buffer

    // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
    // other errors which need to be checked (as they disable receiver) are
    //dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
    dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);


    result = dwt_read32bitoffsetreg(0x0E,0);//SYS_MASK_ID
    result++;

    result = dwt_read32bitoffsetreg(0x0F,0);//SYS_STATUS_ID
    result++;

    dwt_setcallbacks(instTXcallback, instRXcallback);

    //gInst->stateMachine.monitor = 0;

    gInst->stateMachine.responseTO = -10; //initialise
    gInst->tmAndDelays.delayedReplyTime = 0;

    return 0 ;
}

// -------------------------------------------------------------------------------------------------------------------
//
// Return the Device ID register value, enables higher level validation of physical device presence
//

uint32_t instReadDeviceID(void)
{
    return dwt_readdevid() ;
}


// -------------------------------------------------------------------------------------------------------------------
//
// function to allow application configuration be passed into instance and affect underlying device operation
//
void instance_config(const instanceConfig_t *config, const sfConfig_t *sfConfig)
{
    uint32_t power = 0;
    uint8_t otprev ;

    gInst->setting.configData.chan = config->channelNumber ;
    gInst->setting.configData.rxCode =  config->preambleCode ;
    gInst->setting.configData.txCode = config->preambleCode ;
    gInst->setting.configData.prf = config->pulseRepFreq ;
    gInst->setting.configData.dataRate = config->dataRate ;
    gInst->setting.configData.txPreambLength = config->preambleLen ;
    gInst->setting.configData.rxPAC = config->pacSize ;
    gInst->setting.configData.nsSFD = config->nsSFD ;
    gInst->setting.configData.phrMode = DWT_PHRMODE_STD ;
    gInst->setting.configData.sfdTO = config->sfdTO;

    //the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
    //smartPowerEn should be set based on the frame length, but we can also use data rate.
    if( DWT_BR_6M8 == gInst->setting.configData.dataRate && 0 == config->txPower)
    {
        gInst->setting.configData.smartPowerEn = 1;
    }
    else
    {
        gInst->setting.configData.smartPowerEn = 0;
    } 

    //configure the channel parameters
    dwt_configure((dwt_config_t*)&(gInst->setting.configData), DWT_LOADXTALTRIM) ;

    gInst->setting.configTX.PGdly = txSpectrumConfig[config->channelNumber].PGdelay ;

    //firstly check if there are calibrated TX power value in the DW1000 OTP
    power = dwt_getotptxpower(config->pulseRepFreq, gInst->setting.configData.chan);

    if((power == 0x0) || (power == 0xFFFFFFFF)) //if there are no calibrated values... need to use defaults
    {
        if(1 == gInst->setting.configData.smartPowerEn)
        {
            dwt_setsmarttxpower(1);
            power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq - DWT_PRF_16M];
        }
        else
        {
            dwt_setsmarttxpower(0);
#if DW_MAX_POWER==1
            power = txManualSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq - DWT_PRF_16M];
#else
            power = maxSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq - DWT_PRF_16M];
#endif
            power = (power & 0xff0000ff) | (config->txPower<<8) | (config->txPower<<16);
        }
    }

    //Configure TX power
    gInst->setting.configTX.power = power;

    gInst->setting.configTX.power = 0x39393939;
    gInst->setting.configTX.power = 0x1F1F1F1F;

    //configure the tx spectrum parameters (power and PG delay)
    dwt_configuretxrf((dwt_txconfig_t*)&gInst->setting.configTX);

    otprev = dwt_otprevision() ;  // this revision tells us how OTP is programmed.

    if ((2 == otprev) || (3 == otprev))  // board is calibrated with TREK1000 with antenna delays set for each use case)
    {
        uint8_t mode = 1;
        uint8_t chanindex = 0;

        gInst->setting.txAntennaDelay = dwt_getTREKOTPantennadelay(mode,
                                                gInst->setting.configData.chan,
                                                gInst->setting.configData.dataRate) ;

        // if nothing was actually programmed then set a reasonable value anyway
        if ((gInst->setting.txAntennaDelay == 0)
            || (gInst->setting.txAntennaDelay == 0xffff))
        {
            if(gInst->setting.configData.chan == 5)
            {
                chanindex = 1;
            }

            gInst->setting.txAntennaDelay = rfDelaysTREK[chanindex];
        }
    }
    else // assume it is older EVK1000 programming.
    {
        //get the antenna delay that was read from the OTP calibration area
        gInst->setting.txAntennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;

        // if nothing was actually programmed then set a reasonable value anyway
        if ((gInst->setting.txAntennaDelay == 0)
            || (gInst->setting.txAntennaDelay == 0xffff))
        {
            gInst->setting.txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
        }
    }

    // -------------------------------------------------------------------------------------------------------------------
    // set the antenna delay, we assume that the RX is the same as TX.
    dwt_setrxantennadelay(gInst->setting.txAntennaDelay);
    dwt_settxantennadelay(gInst->setting.txAntennaDelay);

    gInst->setting.rxAntennaDelay = gInst->setting.txAntennaDelay;

    if(config->preambleLen == DWT_PLEN_64) //if preamble length is 64
    {
        BSP_DWM_SPI_FastRate(0); //reduce SPI to < 3MHz
        dwt_loadopsettabfromotp(0);
        BSP_DWM_SPI_FastRate(1); //increase SPI to max
    }


    instSetTagSleepDelay(sfConfig->pollSleepDly); //set the Tag sleep time
    gInst->setting.sframePeriod = sfConfig->sfPeriod;
    gInst->setting.slotPeriod = sfConfig->slotPeriod;
    gInst->tmAndDelays.tagSleepRnd = sfConfig->slotPeriod;
    gInst->setting.numSlots = sfConfig->numSlots;

    //set the default response delays
    instSetReplyDelay(sfConfig->replyDly);

    //PA control
    //dwt_setGPIOforEXTPA();
}

// -------------------------------------------------------------------------------------------------------------------
// function to set the tag sleep time (in ms)
//
void instSetTagSleepDelay(int sleepdelay) //sleep in ms
{
    gInst->tmAndDelays.tagSleepTime_ms = sleepdelay; //subtract the micro system delays (time it takes to switch states etc.)
}

//range result of anchors
void SetTofResult(uint32_t addr, uint32_t value)
{
    int index = addr & ANCHOR_LIST_INDEX_MASK;
    gInst->twrResult.tofArray[index].address = addr;
    gInst->twrResult.tofArray[index].value = value;
}


uint32_t GetTofResult(uint32_t address)
{
    int index = address & ANCHOR_LIST_INDEX_MASK;
    return gInst->twrResult.tofArray[index].value;
}


void ClearTofResult(uint32_t address)
{
    int index ;
    for(index=0; index < MAX_ANCHOR_LIST_SIZE; index++)
    {
        if(address == gInst->twrResult.tofArray[index].address)
        {
            gInst->twrResult.tofArray[index].address = BROADCAST_ADDR;
            gInst->twrResult.tofArray[index].value = INVALID_TOF;
        }
    }
}

#define SLOT_COUNT_IN_GROUP                 (32)
#define SLOT_ALL_IN_GROUP                   (0xffffffff)
#define SLOT_FLAG_COUNT                     (100)
volatile uint16_t slotIndex[MAX_TAG_LIST_SIZE] = {0};
volatile uint16_t slotAddress[SLOT_FLAG_COUNT] = {0};
volatile int8_t slotDelay[SLOT_FLAG_COUNT] = {0};
volatile int slotIdx = 0;
volatile int loop =0;
int instRangeAllocateSlot(uint32_t address)
{
    int i,j,find = 0;
    slotIdx = 2;

    if (slotIndex[address])
    {
        if (slotDelay[slotIndex[address]] > 0)
        {
            slotDelay[slotIndex[address]] = 10;
            return slotIndex[address];
        }
    }
    

    while (loop < SLOT_FLAG_COUNT)
    {
        if (slotDelay[slotIdx] <= 0)
        {
            slotIndex[address] = slotIdx;
            slotAddress[slotIdx] = address;
            slotDelay[slotIdx] = 10;
            return slotIndex[address];
        }

        slotIdx += 2;
        if (slotIdx >= 100)
        {
            slotIdx = 1;
        }
        loop++;
    }

    return address % MAX_TAG_LIST_SIZE;
}

int instRangeSlotUpdate()
{
    for (int i=0;i<SLOT_FLAG_COUNT;i++)
    {
        slotDelay[i]--;
        if (slotDelay[i] <= 0)
        {
            slotDelay[i] = 0;
            slotIndex[slotAddress[i]] = 0;
            slotAddress[i] = 0;
        }
    }
}


int instGetRangeInformIndex(uint32_t address, uint32_t alloacated)
{
    //return (address & TAG_LIST_INDEX_MASK);
    uint16_t find = 0;
    uint16_t step = 0;//initiallize
    int availableIndex = -1;
    int index =  (address % MAX_TAG_LIST_SIZE);
    if (gInst->rangeInform[index].address == address)
    { //find the allocated slot
        find = 2;
    }
    else if (gInst->rangeInform[index].address == BROADCAST_ADDR)
    {
        if(!alloacated)
        {//find slot for new tag
            find = 1;
        }
        if(availableIndex < 0)
        {//record the available slot
            availableIndex = index;
        }
    }

    switch(find)
    {
        case 1:
            gInst->rangeInform[index].address = address;
            gInst->rangeInform[index].slot = instRangeAllocateSlot(address);
        case 2:
            gInst->rangeInform[index].flag = 1;
            gInst->rangeInform[index].slot = instRangeAllocateSlot(address);
            gInst->rangeInform[index].tickUpdate = HAL_GetTick();
            break;
    }
    return index;
}

//range from Tag to anc
void SetTagTofResult(uint32_t address, uint32_t value  )
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        gInst->rangeInform[index].address = address;
        gInst->rangeInform[index].tofOfTags = value;
    }
}


uint32_t GetTagTofResult(uint32_t address)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >=0)
    {
        return gInst->rangeInform[index].tofOfTags;
    }
    else
    {
        return INVALID_TOF;
    }
}


void ClearTagTofResult(uint32_t address)
{
    int index ;
    for(index=0; index < MAX_TAG_LIST_SIZE; index++)
    {
        if(address == gInst->rangeInform[index].address)
        {
            gInst->rangeInform[index].tofOfTags = INVALID_TOF;
        }
    }
}


//rangeNum from tag
uint32_t instGetTagRangeNum(uint32_t address)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index > 0)
    {
        return gInst->rangeInform[index].rangeNumA;
    }
    else
    {
        return 0xFF;
    }
    
}


void instSetTagRangeNum(uint32_t address, uint32_t num)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index > 0)
    {
        gInst->rangeInform[index].rangeNumA = num;
    }
}


//response flag for tag
void instInitTagResponseFlag(uint32_t address, uint32_t rangeNum)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        int respIndex = rangeNum & RESP_RECORD_MASK;
        gInst->rangeInform[index].rxRespsIdx = respIndex;
        gInst->rangeInform[index].rxResps[respIndex] = 0;
    }
}


int32_t instGetTagResponseFlag(uint32_t address)
{
    int index = instGetRangeInformIndex(address, 1);
    if (index >= 0)
    {
        int reseIndex = gInst->rangeInform[index].rxRespsIdx;
        return gInst->rangeInform[index].rxResps[reseIndex];
    }
    else
    {
        return -1;
    }
}


void instIncTagResponseFlag(uint32_t address)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        int reseIndex = gInst->rangeInform[index].rxRespsIdx;
        gInst->rangeInform[index].rxResps[reseIndex]++;
    }
}


void instClearTagResponseFlag(uint32_t address)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        int reseIndex = gInst->rangeInform[index].rxRespsIdx;
        if(gInst->rangeInform[index].rxResps[reseIndex] >= 0)
        {
            gInst->rangeInform[index].rxResps[reseIndex] *= -1;
            if(gInst->rangeInform[index].rxResps[reseIndex] == 0) //as A0 will have this as 0 when ranging to A1
            {
                gInst->rangeInform[index].rxResps[reseIndex] = -1 ;
            }
        }
    }
}


//Tag command
void SaveTagCommand(uint8_t* buf, uint32_t len)
{
    if(len < 6)return;

    uint32_t address = buf[0] + (buf[1]<<8);
    int count = instGetRangeInformIndex(address, 0);
    if (count >= 0)
    {
        int index = 2;
        gInst->rangeInform[count].address = address;
        gInst->rangeInform[count].tagCommand.control = buf[index++];
        gInst->rangeInform[count].tagCommand.command = buf[index++];
        gInst->rangeInform[count].tagCommand.reservered1 = buf[index++];
        gInst->rangeInform[count].tagCommand.reservered2 = buf[index++];
        gInst->rangeInform[count].tagCommand.reservered3 = buf[index++];
        gInst->rangeInform[count].tagCommand.reservered4 = buf[index++];
    }
}


int GetTagCommand(uint16_t sourceAddress, stTagControlCommand* cmd)
{
    int index = 0;
    memset((void*)cmd, 0, sizeof(stTagControlCommand));
    for(;index< MAX_TAG_LIST_SIZE;index++)
    {
        if( sourceAddress ==  gInst->rangeInform[index].address )
        {
            memcpy((void*)cmd, (const void*)(&gInst->rangeInform[index].tagCommand), sizeof(stTagControlCommand));
            memset((void*)(&gInst->rangeInform[index].tagCommand), 0, sizeof(stTagControlCommand));//after cmd is used, clear it!!!
            break;
        }
    }
    if( index >= MAX_TAG_LIST_SIZE )
    {
        index = -1;
    }
    return index;
}

//tag status
int GetTagStatus(uint32_t address, stTagStatus* status)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        memcpy((void*)status, (const void*)(&gInst->rangeInform[index].tagStatus), sizeof(stTagStatus));//clear tagStatus
        memset((void*)(&gInst->rangeInform[index].tagStatus), 0, sizeof(stTagStatus));//clear tagStatus
    }
    return 0;
}

void SaveTagStatus(uint16_t address, uint8_t* messageData)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >= 0)
    {
        gInst->rangeInform[index].address = address;
        gInst->rangeInform[index].flag = 1;
        //Get status from TAG POLL message
        gInst->rangeInform[index].tagStatus.status = messageData[POLL_TAG_STATUS];
        gInst->rangeInform[index].tagStatus.power = messageData[POLL_TAG_POWER];
        gInst->rangeInform[index].tagStatus.heartRate = messageData[POLL_TAG_HEARTBEAT];
        gInst->rangeInform[index].tagStatus.reserved1 = messageData[POLL_TAG_RESV1];
        gInst->rangeInform[index].tagStatus.reserved2= messageData[POLL_TAG_RESV2];
    }
}

int16_t GetTagSlot(uint16_t address)
{
    int index = instGetRangeInformIndex(address, 0);
    if (index >=0 )
    {
        return gInst->rangeInform[index].slot;
    }
    else
    {
        return address % 4;
    }
}

int instGetLCount(void) //get count of ranges used for calculation of lt avg
{
    int x = gInst->tmAndDelays.longTermRangeCount;
    return (x);
}

uint16_t instGetIdistAddress(int idx) //get instantaneous range
{
    return gInst->twrResult.distance[idx].address;
}

double instGetIdist(int idx) //get instantaneous range
{
    double x ;
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
    x = gInst->twrResult.distance[idx].distanceBias;
    return (x);
}

double instGetIdistRaw(int idx) //get instantaneous range (uncorrected)
{
    double x ;
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
    x = gInst->twrResult.distance[idx].distanceRaw;
    return (x);
}

int instGetIdist_mm(int idx) //get instantaneous range
{
    int x ;
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
    x = (int)(gInst->twrResult.distance[idx].distanceBias * 1000);
    return (x);
}

int instGetIdistRaw_mm(int idx) //get instantaneous range (uncorrected)
{
    int x ;
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
    x = (int)(gInst->twrResult.distance[idx].distanceRaw * 1000);
    return (x);
}

void instBack2Anchor(instance_data_t *inst)
{
    //stay in RX and behave as anchor
    inst->stateMachine.testAppState = TA_RXE_WAIT ;
    dwt_setrxtimeout(0);
    dwt_setpreambledetecttimeout(0);
    dwt_setrxaftertxdelay(0);
}


#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
void instProcessRXtimeout(instance_data_t *inst)
{
    inst->stateMachine.done = INST_NOT_DONE_YET;
    inst->stateMachine.testAppState = TA_RXE_WAIT ;   // wait for next frame
    dwt_setrxtimeout(0);//Close timeout
    //timeout - disable the radio (if using SW timeout the rx will not be off)
    //dwt_forcetrxoff() ;
}

//
// NB: This function is called from the (TX) interrupt handler
//
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
void instTXcallback(const dwt_callback_data_t *txd)
{
    uint8_t txTimeStamp[TIMESTAME_LENGTH] = {0, 0, 0, 0, 0};
    uint8_t txevent = txd->event;
    event_data_t dw_event;

    dw_event.uTimeStamp = portGetTickCnt();

    if(txevent == DWT_SIG_TX_DONE)
    {
        //NOTE - we can only get TX good (done) while here
        //dwt_readtxtimestamp((uint8_t*) &instance_data[instance].txu.txTimeStamp);

        dwt_readtxtimestamp(txTimeStamp) ;
        dw_event.timeStamp32l = (uint32_t)txTimeStamp[0] + ((uint32_t)txTimeStamp[1] << 8) + ((uint32_t)txTimeStamp[2] << 16) + ((uint32_t)txTimeStamp[3] << 24);
        dw_event.timeStamp = txTimeStamp[4];
        dw_event.timeStamp <<= 32;
        dw_event.timeStamp += dw_event.timeStamp32l;
        dw_event.timeStamp32h = ((uint32_t)txTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);

        //TX timestamp
        gInst->tmAndDelays.txu.txTimeStamp = dw_event.timeStamp;

        dw_event.rxLength = gInst->msgFrame.psduLength;
        dw_event.type =  0;
        dw_event.typePend =  0;
        dw_event.typeSave = DWT_SIG_TX_DONE;

        memcpy((uint8_t *)&dw_event.msgUnion.frame[0], (uint8_t *)&gInst->msgFrame.content, gInst->msgFrame.psduLength);

        instPutEvent(dw_event, DWT_SIG_TX_DONE);

        trxStage = RTX_IDLE;
    }
    else if(txevent == DWT_SIG_TX_AA_DONE)
    {
        //auto ACK confirmation
        dw_event.rxLength = 0;
        dw_event.type =  0;
        dw_event.typeSave = DWT_SIG_TX_AA_DONE;

        instPutEvent(dw_event, DWT_SIG_TX_AA_DONE);
    }
    //gInst->stateMachine.monitor = 0;
}

/**
 * @brief this function either enables the receiver (delayed)
 *
 **/
void ancEnableRX(void)
{
    //subtract preamble length
    dwt_setdelayedtrxtime(gInst->tmAndDelays.delayedReplyTime - gInst->setting.fixedReplyDelayAncP) ;
    if(dwt_rxenable(DWT_START_RX_DELAYED)) //delayed rx
    {
        //if the delayed RX failed - time has passed - do immediate enable
        dwt_setrxtimeout((uint16_t)gInst->setting.fwtoTimeAnc_sy*2); //reconfigure the timeout before enable
        //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
        dwt_rxenable(DWT_START_RX_IMMEDIATE);
        trxStage = RTX_RX;
        dwt_setrxtimeout((uint16_t)gInst->setting.fwtoTimeAnc_sy); //restore the timeout for next RX enable
    }
}

/**
 * @brief this function either re-enables the receiver (delayed or immediate) or transmits the response frame
 *
 * @param the sourceAddress is the address of the sender of the current received frame
 *
 */
#ifdef __GNUC__
#pragma GCC optimize ("O0")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
uint8_t ancTXorRXReEnable(uint16_t sourceAddress)
{
    uint8_t typePend = DWT_SIG_DW_IDLE;
    //int sendResp = 0;

    if(gInst->stateMachine.responseTO == 0) //go back to RX without TO - ranging has finished. (wait for Final but no TO)
    {
        dwt_setrxtimeout(0); //reconfigure the timeout
        dwt_setpreambledetecttimeout(0);
    }

    //configure delayed reply time (this is incremented for each received frame) it is timed from Poll rx time
    gInst->tmAndDelays.delayedReplyTime += (gInst->setting.fixedReplyDelayAnc >> 8);

    //this checks if to send a frame
    if( (gInst->stateMachine.responseTO + gInst->setting.shortAdd_idx) == NUM_EXPECTED_RESPONSES)
    {
        //response is expected
        gInst->stateMachine.wait4ack = DWT_RESPONSE_EXPECTED; //re has/will be re-enabled
        dwt_setdelayedtrxtime(gInst->tmAndDelays.delayedReplyTime) ;
        
        if(dwt_starttx(DWT_START_TX_DELAYED | gInst->stateMachine.wait4ack))
        //if(dwt_starttx(DWT_START_TX_IMMEDIATE | gInst->stateMachine.wait4ack))
        {//if TX has failed - we need to re-enable RX for the next response or final reception...
            dwt_setrxaftertxdelay(0);
            gInst->stateMachine.wait4ack = 0; //clear the flag as the TX has failed the TRX is off
            gInst->tmAndDelays.delayedReplyTime += 2*(gInst->setting.fixedReplyDelayAnc >> 8); //to take into account W4R
            ancEnableRX();
            typePend = DWT_SIG_RX_PENDING ;
            instDebug.ancRespLateCount++;
            trxStage = RTX_RX;
        }
        else
        {
            gInst->tmAndDelays.delayedReplyTime += (gInst->setting.fixedReplyDelayAnc >> 8); //to take into account W4R
            typePend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.

            instDebug.andRespSentCount++;
            trxStage = RTX_TX_THEN_RX;
        }
    }
    else //stay in receive
    {
        if(sourceAddress == 0)
        { //we got here after RX error, as we don't need to TX, we just enable RX
            dwt_setrxtimeout(0);
            dwt_rxenable(DWT_START_RX_IMMEDIATE);
            trxStage = RTX_RX;
        }
        else
        {
            if(gInst->stateMachine.responseTO > 0) //go back to RX without TO - ranging has finished. (wait for Final but no TO)
            {
                ancEnableRX();//??
            }
            else
            {
                dwt_rxenable(DWT_START_RX_IMMEDIATE);
                trxStage = RTX_RX;
            }
        }

        typePend = DWT_SIG_RX_PENDING ;
    }

    return typePend;
}

/**
 * @brief this function handles frame error event, it will either signal TO or re-enable the receiver
 */
void handle_error_unknownFrame(event_data_t dw_event)
{
    //re-enable the receiver (after error frames as we are not using auto re-enable
    //for ranging application rx error frame is same as TO - as we are not going to get the expected frame

    instDebug.errFrameCount++;
    instDebug.toBeHandled = TRUE;
    instDebug.type = dw_event.typeSave;

    //if we are participating in the ranging (i.e. Poll was received)
    //and we get an rx error (in one of the responses)
    //need to consider this as a timeout as we could be sending our response next and
    //the applications needs to know to change the state
    //
    if(gInst->stateMachine.responseTO > 0)
    {
        gInst->stateMachine.responseTO--;

        //send a response or re-enable rx
        dw_event.typePend = ancTXorRXReEnable(0);
        dw_event.type = 0;
        dw_event.typeSave = 0x40 | DWT_SIG_RX_TIMEOUT;
        dw_event.rxLength = 0;

        instPutEvent(dw_event, DWT_SIG_RX_TIMEOUT);

        //dwt_setrxtimeout(0); //reconfigure the timeout
        //dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
    }
    else
    {
        dwt_setrxtimeout(0); //reconfigure the timeout
        dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
        trxStage = RTX_RX;
    }
}

/**
 * @brief this function prepares and writes the anchor to tag response frame into the TX buffer
 * it is called after anchor receives a Poll from a tag
 */
void ancPrepareResponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp)
{
    uint16_t frameLength = 0;
    uConverterUint2Bytes time = {0};
    uint32_t tofvalue=0;

    gInst->msgFrame.psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
    memcpy((void*)&gInst->msgFrame.content.dstAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
    gInst->msgFrame.content.srcAddr[0] = gInst->setting.eui64[0];
    gInst->msgFrame.content.srcAddr[1] = gInst->setting.eui64[1];
    // Write calculated TOF into response message (get the previous ToF+range number from that tag)
    tofvalue = GetTagTofResult(sourceAddress);
    memcpy((void*)&(gInst->msgFrame.content.messageData[TOFR]), (void*)&tofvalue, 4);
    memcpy((void*)&gInst->msgFrame.content.messageData[TAGADDR], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //get the previous range number
#if 1
    //TagCommand: From ANC to TAG message
    stTagControlCommand cmd = {0};
    time.u32Data = g_stTimeValue.timeValue;
    int index = GetTagCommand(sourceAddress, &cmd);
    gInst->msgFrame.content.messageData[RESP_CONTROL] = cmd.control; //Control byte
    if(cmd.command != 0) //Ext Cmd
    {//command
        gInst->msgFrame.content.messageData[RESP_CMD] = cmd.command; //command byte

        time.u8Data[0] = cmd.reservered1;
        time.u8Data[1] = cmd.reservered2;
        time.u8Data[2] = cmd.reservered3;
        time.u8Data[3] = cmd.reservered4;
        gInst->msgFrame.content.messageData[RESP_TIME1] = time.u8Data[0]; //Time1
        gInst->msgFrame.content.messageData[RESP_TIME2] = time.u8Data[1]; //Time2
        gInst->msgFrame.content.messageData[RESP_TIME3] = time.u8Data[2]; //Time3
        gInst->msgFrame.content.messageData[RESP_TIME4] = time.u8Data[3]; //Time4
    }
    else if (time.u32Data > 0) //Timeset
    {//Timeset
        gInst->msgFrame.content.messageData[RESP_CMD] = EXTCMD_TIMESET; //set framePeroid

        gInst->msgFrame.content.messageData[RESP_TIME1] = time.u8Data[0]; //Time1
        gInst->msgFrame.content.messageData[RESP_TIME2] = time.u8Data[1]; //Time2
        gInst->msgFrame.content.messageData[RESP_TIME3] = time.u8Data[2]; //Time3
        gInst->msgFrame.content.messageData[RESP_TIME4] = time.u8Data[3]; //Time4
    }
    else // Superframe Peroid
    {//Peroid
        gInst->msgFrame.content.messageData[RESP_CMD] = EXTCMD_ANCFRAMEPEROID; //set framePeroid
        time.u32Data = g_stExtandCommand.param;
        gInst->msgFrame.content.messageData[RESP_TIME1] = time.u8Data[0]; //Time1
        gInst->msgFrame.content.messageData[RESP_TIME2] = time.u8Data[1]; //Time2
        gInst->msgFrame.content.messageData[RESP_TIME3] = time.u8Data[2]; //Time3
        gInst->msgFrame.content.messageData[RESP_TIME4] = time.u8Data[3]; //Time4
    }
#endif
    instSetTagRangeNum(sourceAddress, frame[POLL_RNUM + fcode_index]);
    gInst->msgFrame.content.messageData[TOFRN] = frame[POLL_RNUM + fcode_index]; //get the previous range number

    gInst->msgFrame.content.seqNum = gInst->msgFrame.frameSN++;

    //we have our range - update the own mask entry...
    if(tofvalue != INVALID_TOF) //check the last ToF entry is valid and copy into the current array
    {
        SetTofResult(gInst->setting.instanceAddress16, tofvalue);
    }
    else	//reset response mask
    {
        ClearTofResult(gInst->setting.instanceAddress16);
    }
    
    //set the delayed rx on time (the final message will be sent after this delay)
    dwt_setrxaftertxdelay(gInst->setting.ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
    //dwt_setrxaftertxdelay(0);

    //���еĻ�վ����������ʱ�䣬�ֻ�����ɽ��յ�
    {
        int error = 0;
        int currentSlotTime = 0;
        int expectedSlotTime = 0;
        int slot =-1;
        //find the time in the current superframe
        currentSlotTime = uTimeStamp % gInst->setting.sframePeriod;

        //this is the slot time the poll should be received in (Mask 0x07 for the 8 MAX tags we support in TREK)
        slot = GetTagSlot(sourceAddress);
        expectedSlotTime = slot * TA_SLOT_STEP * gInst->setting.slotPeriod; //

        error = expectedSlotTime - currentSlotTime;

        if(error < (-(gInst->setting.sframePeriod>>1)) //if error is more negative than 0.5 period, add whole period to give up to 1.5 period sleep
            && error < -12)
        {
            gInst->tmAndDelays.tagSleepCorrection = (gInst->setting.sframePeriod + error);
        }
        else //the minimum Sleep time will be 0.5 period
        {
            gInst->tmAndDelays.tagSleepCorrection = error;
        }
        gInst->msgFrame.content.messageData[RES_TAG_SLP0] = gInst->tmAndDelays.tagSleepCorrection & 0xFF ;
        gInst->msgFrame.content.messageData[RES_TAG_SLP1] = (gInst->tmAndDelays.tagSleepCorrection >> 8) & 0xFF;
    }
    
    
    gInst->msgFrame.content.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)

    //write the TX data
    dwt_writetxfctrl(frameLength, 0);
    dwt_writetxdata(frameLength, (uint8_t *)  &gInst->msgFrame.content, 0);	// write the frame data
}

/**
 * @brief this is the receive event callback handler, the received event is processed and the instance either
 * responds by sending a response frame or re-enables the receiver to await the next frame
 * once the immediate action is taken care of the event is queued up for application to process
 */
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
void instRXcallback(const dwt_callback_data_t *rxd)
{
    uint8_t rxTimeStamp[TIMESTAME_LENGTH]  = {0, 0, 0, 0, 0};

    uint8_t rxd_event = 0;
    uint8_t fcode_index  = 0;
    uint8_t srcAddr_index = 0;
    event_data_t dw_event;

    //microcontroller time at which we received the frame
    dw_event.uTimeStamp = portGetTickCnt();

    //if we got a frame with a good CRC - RX OK
    if( rxd->event == DWT_SIG_RX_OKAY
        || rxd->event == DWT_SIG_RX_PHR_ERROR
        )
    {
        dw_event.rxLength = rxd->datalength;

        //need to process the frame control bytes to figure out what type of frame we have received
        if( ((rxd->fctrl[0] == 0x41) || (rxd->fctrl[0] == 0x61))
            && ((rxd->fctrl[1] & 0xCC) == 0x88)) //short address
        {
            fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
            srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
            rxd_event = DWT_SIG_RX_OKAY;
        }
        else
        {
            rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
        }

        //read RX timestamp
        dwt_readrxtimestamp(rxTimeStamp) ;
        dwt_readrxdata((uint8_t *)&dw_event.msgUnion.frame[0], rxd->datalength, 0);  // Read Data Frame
        dw_event.timeStamp32l =  (uint32_t)rxTimeStamp[0] + ((uint32_t)rxTimeStamp[1] << 8) + ((uint32_t)rxTimeStamp[2] << 16) + ((uint32_t)rxTimeStamp[3] << 24);
        dw_event.timeStamp = rxTimeStamp[4];
        dw_event.timeStamp <<= 32;
        dw_event.timeStamp += dw_event.timeStamp32l;
        dw_event.timeStamp32h = ((uint32_t)rxTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);

        dw_event.type = 0; //type will be added as part of adding to event queue
        dw_event.typeSave = rxd_event;
        dw_event.typePend = DWT_SIG_DW_IDLE;

        //if Listener then just report the received frame to the instance (application)
        if(rxd_event == DWT_SIG_RX_OKAY) //Process good/known frame types
        {
            uint16_t sourceAddress = (((uint16_t)dw_event.msgUnion.frame[srcAddr_index+1]) << 8) + dw_event.msgUnion.frame[srcAddr_index];

            //check if this is a TWR message (and also which one)
            switch(dw_event.msgUnion.frame[fcode_index])
            {
                //poll message from an anchor
                case RTLS_DEMO_MSG_ANCH_POLL:
                {
                } break;

                case RTLS_DEMO_MSG_TAG_POLL:
                {
                    instInitTagResponseFlag(sourceAddress, dw_event.msgUnion.frame[POLL_RNUM + fcode_index]);

                    float uwb_rssi = dwGetReceivePower();
                    gInst->rangeInform[sourceAddress].tagStatus.txRssi = uwb_rssi;
                    //gInst->stateMachine.isAncResp = 0;

                    //prepare the response and write it to the tx buffer
                    ancPrepareResponse(sourceAddress, srcAddr_index, fcode_index, &dw_event.msgUnion.frame[0], dw_event.uTimeStamp);

                    gInst->tmAndDelays.tagPollRxTime = dw_event.timeStamp ; //save Poll's Rx time

                    gInst->tmAndDelays.delayedReplyTime = dw_event.timeStamp32h /*+ (instance_data[0].fixedReplyDelayAnc >> 8)*/ ;
                    gInst->stateMachine.responseTO = NUM_EXPECTED_RESPONSES; //set number of expected responses to 3 (from other anchors)
                    /*if(gInst->setting.shortAdd_idx==2)
                    {
                    gInst->tmAndDelays.delayedReplyTime += 3 * (gInst->setting.fixedReplyDelayAnc >> 8); //to take into account W4R
                    //dwt_setrxtimeout((uint16_t)gInst->setting.fwtoTime_sy * NUM_EXPECTED_RESPONSES); //reconfigure the timeout for response
                    dwt_setrxtimeout(gInst->setting.pollRx2FinalRxDelay);
                    }
                    else*/
                    {
                        dwt_setrxtimeout((uint16_t)gInst->setting.fwtoTimeAnc_sy); //reconfigure the timeout for response
                    }

                    dw_event.typePend = ancTXorRXReEnable(gInst->setting.instanceAddress16);

                    //clear ToF ..
                    ClearTagTofResult(sourceAddress);

                    memset((void*)&tagFinalRecord, 0, sizeof(stTagFinalRecord));
                    ancRespCount = 0;
                    memset((void*)&ancRespRecords, 0 ,sizeof(ancRespRecords));

                    instDebug.ancPollCount++;

                    //sprintf((char*)debugBuffer,"Rssi, rn=%d, tx=%f \r\n", gInst->rangeInform[sourceAddress].rangeNumA, uwb_rssi);
                    //DebugMessage(debugBuffer);
                } break;

                //we got a response from a "responder" (anchor)
                case RTLS_DEMO_MSG_ANCH_RESP2:
                    break;
                

                case RTLS_DEMO_MSG_ANCH_RESP:
                {
                    uint16_t tagAddress = (uint16_t) (dw_event.msgUnion.frame[fcode_index + TAGADDR] + (dw_event.msgUnion.frame[fcode_index + TAGADDR + 1]<<8));
                    //got a response... (check if we got a Poll with the same range number as in this response)
                    if(RTLS_DEMO_MSG_ANCH_RESP == dw_event.msgUnion.frame[fcode_index])
                    {
                        //if((gInst->setting.shortAdd_idx != 2)&&
                        if((instGetTagResponseFlag(tagAddress) >= 0) //we got the poll else ignore this response
                            && (gInst->stateMachine.responseTO > 0)	) //if responseTO == 0 we have already received all of the responses - meaning should not be here => error
                        {
                            instIncTagResponseFlag(tagAddress); //increment the number of responses received
                            gInst->stateMachine.responseTO--;

                            //send a response or re-enable rx
                            dw_event.typePend = ancTXorRXReEnable(sourceAddress);
                        }
                        else //like a timeout (error) ...
                        {
                            //send a response or re-enable rx
                            dwt_setrxtimeout(0); //reconfigure the timeout
                            dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
                            trxStage = RTX_RX;
                            dw_event.typePend = DWT_SIG_RX_PENDING ;
                        }
                    }

                    int currentRangeNum = dw_event.msgUnion.frame[fcode_index + TOFRN];
                    if(instGetTagRangeNum(tagAddress) == currentRangeNum)
                    {
                        uint16_t src_addr = 0;
                        uint32_t src_tof = 0;
                        memcpy((void*)&src_addr, &dw_event.msgUnion.frame[srcAddr_index], 2);
                        memcpy((void*)&src_tof, &(dw_event.msgUnion.frame[fcode_index + TOFR]), TIMEOFFLY_LENGTH);
                        SetTofResult(src_addr, src_tof);

                        instDebug.ancRespCounts[sourceAddress & ANCHOR_LIST_INDEX_MASK]++;

                        ancRespRecords[ancRespCount].rangNum = dw_event.msgUnion.frame[fcode_index + TOFRN];
                        memcpy((void*)&ancRespRecords[ancRespCount].address, &dw_event.msgUnion.frame[srcAddr_index], 2);
                        memcpy((void*)&ancRespRecords[ancRespCount].tof, &dw_event.msgUnion.frame[fcode_index + TOFR], TIMEOFFLY_LENGTH);
                        ancRespCount++;
                    }
                    else
                    {
                        ClearTofResult(sourceAddress);

                        instDebug.ancRespFailsCounts[sourceAddress & ANCHOR_LIST_INDEX_MASK]++;
                    }
                } break;

                
                case RTLS_DEMO_MSG_TAG_FINAL:
                {
                    int count,index;

                    gInst->tmAndDelays.tagFinalRxTime = dw_event.timeStamp;

                    //reset the response count
                    instClearTagResponseFlag(sourceAddress);

                    float uwb_rssi = dwGetReceivePower();
                    gInst->rangeInform[sourceAddress].tagStatus.txRssiFinal = uwb_rssi;

                    tagFinalRecord.tagAddress = sourceAddress;
                    tagFinalRecord.rangNum = dw_event.msgUnion.frame[fcode_index + POLL_RNUM];
                    for(count = 0; count < NUM_EXPECTED_RESPONSES; count++)
                    {
                        index = fcode_index + RRXT0ADDR + (TIMESTAME_LENGTH + ADDR_BYTE_SIZE_S) * count;

                        memcpy((void*)&tagFinalRecord.ancRespTimes[count].address, (void const*)&(dw_event.msgUnion.frame[index]), ADDR_BYTE_SIZE_S);
                        memcpy((void*)tagFinalRecord.ancRespTimes[count].ancRespRxTime, (void const*)&(dw_event.msgUnion.frame[index + ADDR_BYTE_SIZE_S]), TIMESTAME_LENGTH);
                    }

                    memcpy((void*)tagFinalRecord.tagPollTxTime, (void const*)&(dw_event.msgUnion.frame[fcode_index + PTXT]), TIMESTAME_LENGTH);
                    memcpy((void*)tagFinalRecord.tagFinalTxTime, (void const*)&(dw_event.msgUnion.frame[fcode_index + FTXT]), TIMESTAME_LENGTH);

                    dw_event.typePend = DWT_SIG_DW_IDLE;
                    instDebug.ancFinalCount++;

                    //sprintf((char*)debugBuffer,"Rssi, rn=%d, txFinal=%f\r\n", gInst->rangeInform[sourceAddress].rangeNumA, uwb_rssi);
                    //DebugMessage(debugBuffer);

                    //after final received start receiving next poll
                    ///instSetAntennaDelays(); //this will update the antenna delay if it has changed
                    //instSetTXPower(); // configure TX power if it has changed
                    //dwt_setrxtimeout(0); //reconfigure the timeout
                    //dwt_rxenable(DWT_START_RX_IMMEDIATE);
                } break;
                
                
                case RTLS_DEMO_MSG_ANCH_FINAL:
                //if anchor fall into case below and process the frame
                default:  //process rx frame
                {
                    dw_event.typePend = DWT_SIG_DW_IDLE;
                } break;

            }


            instPutEvent(dw_event, rxd_event);
        }
        else //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type)
        {
            handle_error_unknownFrame(dw_event);
        }
    }
    else if (rxd->event == DWT_SIG_RX_TIMEOUT) //if tag and got TO, then did not get any or some responses - check if need to send final.
    {
        dw_event.typePend = DWT_SIG_DW_IDLE;

        instDebug.rxTimeoutCount++;

        //check if anchor has received all of the responses from other anchors (it could have received only 1 or 2)
        //it's timed out (re-enable rx or tx response)
        if(gInst->stateMachine.responseTO > 0)
        {
            gInst->stateMachine.responseTO--;
            //send a response or re-enable rx
            dw_event.typePend = ancTXorRXReEnable(gInst->setting.instanceAddress16);
        }
        else
        {
            //dwt_setrxtimeout(0); //reconfigure the timeout
            //dwt_rxenable(DWT_START_RX_IMMEDIATE);

            //trxStage = RTX_RX;
        }

        dw_event.type = 0;
        dw_event.typeSave = DWT_SIG_RX_TIMEOUT;
        dw_event.rxLength = 0;
        dw_event.timeStamp = 0;
        dw_event.timeStamp32l = 0;
        dw_event.timeStamp32h = 0;

        instPutEvent(dw_event, DWT_SIG_RX_TIMEOUT);
        //printf("RX timeout while in %d\n", instance_data[instance].testAppState);
    }
    else //assume other events are errors
    {
        dw_event.typeSave = rxd->event;
        handle_error_unknownFrame(dw_event);
    }
}

/**************************************************************************************/
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
int instPeekEvent(void)
{
    return peekEvent((const event_queue*)&(gInst->evtQueue));
}
/*
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
void instSaveEvent(event_data_t newEvent, uint8_t eType)
{
    saveEvent((event_queue*)&(gInst->evtQueue), newEvent, eType);
}

#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
event_data_t instGetSavedEvent(void)
{
	return getSavedEvent((event_queue*)&(gInst->evtQueue));
}
*/
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
void instPutEvent(event_data_t newEvent, uint8_t eType)
{
    putEvent((event_queue*)&(gInst->evtQueue), newEvent, eType);
}

#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
event_data_t* instGetEvent(int x)
{
    return getEvent((event_queue*)&(gInst->evtQueue), x);
}

void instClearEvents(void)
{
    clearEvents((event_queue*)&(gInst->evtQueue));
}

// -------------------------------------------------------------------------------------------------------------------

/**************************************************************************************/
#ifdef __GNUC__
#pragma GCC optimize ("O3")
#elif defined(__ICCARM__)
#pragma optimize=speed high
#endif
int instance_run(void)
{
    int done = INST_NOT_DONE_YET;
    int message = 0;

#if 1
    while(done == INST_NOT_DONE_YET)
    {
        message = instPeekEvent(); //get any of the received events from ISR

        //int state = instance_data[instance].testAppState;
        done = testAppRun((instance_data_t*)gInst, message) ;                                               // run the communications application
        //we've processed message
        message = 0;
        //DebugRun();
    }

    //debugLen = sprintf(debugBuffer,"ENDWILE1 state:%d, %d T:%d\r\n",gInst->stateMachine.testAppState,done,portGetTickCnt());
    //ReportMessage(debugBuffer, debugLen);
#else
    message = instPeekEvent(); //get any of the received events from ISR

    while(done == INST_NOT_DONE_YET)
    {
        //int state = instance_data[instance].testAppState;
        done = testAppRun(gInst, message) ;                                               // run the communications application
        //we've processed message
        message = 0;
        //DebugRun();
    }
#endif

    if(instDebug.toBeHandled == TRUE)
    {
        instDebug.toBeHandled=FALSE;

#if (ANC_RESP_TEST==1)
        {
            debugLen = sprintf(debugBuffer,"ERROR FRAME %d: %d T:%d\r\n",
            instDebug.errFrameCount, instDebug.type, portGetTickCnt());
  #if defined(REPORT2COM)
            ReportMessage((uint8_t*)debugBuffer, debugLen);
  #elif defined(REPORT2WIFI)
            SendMessageWifi((uint8_t*)debugBuffer, debugLen);
  #endif
            SendMessage((uint8_t*)debugBuffer,debugLen);
        }
#endif

    }

    return 0 ;
}


void instClose(void)
{
    //wake up device from low power mode
    //NOTE - in the ARM  code just drop chip select for 200us
    port_SPIx_CS_clear();  //CS low
    Sleep(1);   //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
    port_SPIx_CS_set();  //CS high
    Sleep(5);
    dwt_entersleepaftertx(0); // clear the "enter deep sleep after tx" bit

    dwt_setinterrupt(0xFFFFFFFF, 0); //don't allow any interrupts

}

void instConfigTXPower(uint32_t txpower)
{
    gInst->setting.txPower = txpower ;

    gInst->setting.txPowerChanged = 1;

}

void instSetTXPower(void)
{
    if(gInst->setting.txPowerChanged == 1)
    {
        //Configure TX power
        dwt_write32bitreg(0x1E, gInst->setting.txPower);
        gInst->setting.txPowerChanged = 0;
    }
}

void instConfigAntennaDelays(uint16_t tx, uint16_t rx)
{
    gInst->setting.txAntennaDelay = tx ;
    gInst->setting.rxAntennaDelay = rx ;

    gInst->setting.antennaDelayChanged = 1;
}

void instSetAntennaDelays(void)
{
    if(gInst->setting.antennaDelayChanged == 1)
    {
        dwt_setrxantennadelay(gInst->setting.rxAntennaDelay);
        dwt_settxantennadelay(gInst->setting.txAntennaDelay);

        gInst->setting.antennaDelayChanged = 0;
    }
}


uint16_t instTXAntennaDly(void)
{
    return gInst->setting.txAntennaDelay;
}

uint16_t instRXAntennaDly(void)
{
    return gInst->setting.rxAntennaDelay;
}

uint8_t instValidRanges(void)
{
    return 0;
}