/**
 * @file    decawave_app.c
 * @brief   DecawaveApp application level functions
 * @version 6.0
 * @date 27/06/2017
 */
 
#include "decawave_app.h"
 
// Decadriver
#include "deca_device_api.h"
#include "deca_regs.h"

// Platform dependant includes
#include "spi.h"
#include "sleep.h"
#include "interrupts.h"
#include "GPIO.h"
#include "boards.h"

// misc includes
#include "GlobalDefs.h"

int uwbConfigChanged = 0;
instanceConfig_t instConfig;
ReportRangeType ptr_ReportDistanceMeasurement;
OperationChangeType ptr_GoToRangingOperation;

//**************************************************************************************************
// USER FUNCTIONS
//*************************************************************************************************/

/**@brief Set function used to report ToF via BLE. 
 *
 * @param[in] addr pointer to the report function
 *
 */
void DecawaveApp_SetReportDistanceMeasurementFunction (ReportRangeType addr)
{
	ptr_ReportDistanceMeasurement = addr;
}

/**@brief Set function used to go to Ranging Mode. 
 *
 * @param[in] addr pointer to function
 *
 */
void DecawaveApp_SetGoToRangingModeFunction (OperationChangeType addr)
{
	ptr_GoToRangingOperation = addr;
}

/**@brief Function for initializing the decawave application.
 *
 * @details pre configure decawave depending of the role (TAG, ANCHOR)
 *
 * @param[in] inst pointer to data structure
 *
 */
void DecawaveApp_InitExchange()
{
    switch (instance_data[0].mode)
	{
		case TAG:
		{	
			//wake up decawave
			DecawaveApp_WakeUp ();
			
			//configure UWB signal
			if(uwbConfigChanged)
			{
				// Set operating channel etc
				DecawaveApp_ConfigInstance(&instConfig) ;  

				// set timings
				DecawaveApp_InitTimings();
				uwbConfigChanged = 0;
			}
			
			//allow data, ACK frames;
			dwt_enableframefilter(DWT_FF_NOTYPE_EN); 
		//	dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
			instance_data[0].frameFilteringEnabled = 1 ;
			dwt_setpanid(instance_data[0].panid);				
            dwt_seteui(instance_data[0].eui64);
			
			#if (USING_64BIT_ADDR==0)
			//the short address is assigned by the anchor
			#else
            //set source address into the message structure
            memcpy(&instance_data[0].msg.sourceAddr[0], instance_data[0].eui64, ADDR_BYTE_SIZE_L);
			#endif

            //change to next state - send a Poll message to 1st anchor in the list
            instance_data[0].mode = TAG_TDOA ;
			memcpy(instance_data[0].blinkmsg.tagID, instance_data[0].eui64, ADDR_BYTE_SIZE_L);

			#if (DEEP_SLEEP == 1)
			if (instance_data[0].sleep_en)
			{
				//put device into low power mode
				dwt_entersleep(); //go to sleep
			}
			#endif					
		}
		break;
				
        case ANCHOR:
		{
			//wake up decawave
			DecawaveApp_WakeUp ();
			
			//configure UWB signal
			if(uwbConfigChanged)
			{
				// Set operating channel etc
				DecawaveApp_ConfigInstance(&instConfig) ;  

				// set timings
				DecawaveApp_InitTimings();
				uwbConfigChanged = 0;
			}
			
			DecawaveApp_ClearCounts();
			
			//disable frame filtering
			dwt_enableframefilter(DWT_FF_NOTYPE_EN); 
			instance_data[0].frameFilteringEnabled = 0 ;
            dwt_seteui(instance_data[0].eui64);
            dwt_setpanid(instance_data[0].panid);

			#if (USING_64BIT_ADDR==0)
            uint16 addr = instance_data[0].eui64[0] + (instance_data[0].eui64[1] << 8);
            dwt_setaddress16(addr);
			//set source address into the message structure
			memcpy(&instance_data[0].msg.sourceAddr[0], instance_data[0].eui64, ADDR_BYTE_SIZE_S);
			//set source address into the message structure
			memcpy(&instance_data[0].rng_initmsg.sourceAddr[0], instance_data[0].eui64, ADDR_BYTE_SIZE_S);
			#else
            //set source address into the message structure
            memcpy(&instance_data[0].msg.sourceAddr[0], instance_data[0].eui64, ADDR_BYTE_SIZE_L);
			//set source address into the message structure
			memcpy(&instance_data[0].rng_initmsg.sourceAddr[0], instance_data[0].eui64, ADDR_BYTE_SIZE_L);
			#endif

            // First time anchor listens we don't do a delayed RX
			dwt_setrxaftertxdelay(0);
            dwt_setrxtimeout(0);

			// turn RX on, with/without delay
			DecawaveApp_rxON(&instance_data[0], 0, 0) ;   
		}
		break;
					        
		default:
        break; 
	}
	
	// Enable GPIO interrupt (interupts from decawave)
	GPIO_INT_enable (PIN_DECA_INT);
}

void DecawaveApp_EndExchange (void)
{
	DecawaveApp_EnterSleep();
	
	// Disable GPIO interrupt (interupts from decawave)
	GPIO_INT_disable (PIN_DECA_INT);
}

/**@brief Function for configuring the decawave device
 *
 * @param[in] value		 	new configuration
 *
 */
void DecawaveApp_ChannelConfigure (uint16 value)
{
	uint8 PRF, DR, PLEN, PC, CH, SFD;
	
	PRF 	= (value >> DECA_CONF_PRF_OFFSET) 	& DECA_CONF_PRF_SIZE;
	DR 		= (value >> DECA_CONF_DR_OFFSET) 	& DECA_CONF_DR_SIZE;
	PLEN 	= (value >> DECA_CONF_PLEN_OFFSET) 	& DECA_CONF_PLEN_SIZE;
	PC 		= (value >> DECA_CONF_PC_OFFSET) 	& DECA_CONF_PC_SIZE;
	CH 		= (value >> DECA_CONF_CH_OFFSET) 	& DECA_CONF_CH_SIZE;
	SFD 	= (value >> DECA_CONF_SFD_OFFSET) 	& DECA_CONF_SFD_SIZE;
	
    instConfig.pulseRepFreq = PRF+1;
    instConfig.dataRate = DR;	
    instConfig.preambleCode = PC;
	instConfig.channelNumber = CH;
    instConfig.nsSFD = SFD ;	
	switch(PLEN)
	{
		case 0:
			instConfig.preambleLen = DWT_PLEN_4096;	
			instConfig.pacSize = DWT_PAC64;
		    instConfig.sfdTO =	4096+1+64;
		break;
		
		case 1:
			instConfig.preambleLen = DWT_PLEN_2048;	
			instConfig.pacSize = DWT_PAC64;
			instConfig.sfdTO =	2048+1+64;
		break;
				
		case 2:
			instConfig.preambleLen = DWT_PLEN_1536;	
			instConfig.pacSize = DWT_PAC64;
			instConfig.sfdTO =	1536+1+64;
		break;
						
		case 3:
			instConfig.preambleLen = DWT_PLEN_1024;	
			instConfig.pacSize = DWT_PAC32;
			instConfig.sfdTO =	1024+1+32;
		break;
								
		case 4:
			instConfig.preambleLen = DWT_PLEN_512;	
			instConfig.pacSize = DWT_PAC16;
			instConfig.sfdTO =	512+1+16;
		break;
										
		case 5:
			instConfig.preambleLen = DWT_PLEN_256;	
			instConfig.pacSize = DWT_PAC16;
			instConfig.sfdTO =	256+1+16;
		break;
		
		case 6:
			instConfig.preambleLen = DWT_PLEN_128;	
			instConfig.pacSize = DWT_PAC8;
			instConfig.sfdTO =	(129 + 8 - 8);
		break;
				
		case 7:
			instConfig.preambleLen = DWT_PLEN_64;	
			instConfig.pacSize = DWT_PAC8;
			instConfig.sfdTO =	64+1+8;
		break;
		
		default:
			instConfig.preambleLen = DWT_PLEN_1024;	
			instConfig.pacSize = DWT_PAC32;
			instConfig.sfdTO =	1024+1+32;
		break;
	}	
	
	uwbConfigChanged = 1;
}


//**************************************************************************************************
// INTERNAL FUNCTIONS
//*************************************************************************************************/

/**@brief Function for sending a blink message
 *
 * @details prepare msg and send it immediatly
 *
 */
void DecawaveApp_SendBlinkMsg ()
{	
	if (instance_data[0].mode != TAG_TDOA)
		return;
	
	int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);

	//blink frames with IEEE EUI-64 tag ID    
	instance_data[0].blinkmsg.frameCtrl = 0xC5 ;
	instance_data[0].blinkmsg.seqNum = instance_data[0].frame_sn++;

	// write the frame data
	dwt_writetxdata(flength, (uint8 *)  (&instance_data[0].blinkmsg), 0) ;	
	dwt_writetxfctrl(flength, 0);

	//using wait for response to do delayed receive
	dwt_setrxtimeout((uint16)instance_data[0].fwtoTimeB_sy);  //units are symbols
				
	//set the delayed rx on time (the ranging init will be sent after this delay)
	dwt_setrxaftertxdelay((uint32)instance_data[0].rnginitW4Rdelay_sy);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)

	dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //always using immediate TX and enable delayed RX
}

/**@brief Function for sending a poll message (first msg of a 3W exchange)
 *
 * @details send message and prepare for response
 *
 */
void DecawaveApp_SendPollMsg ()
{
	if (instance_data[0].mode != TAG)
			return;
	
	//NOTE the anchor address is set after receiving the ranging initialisation message

	instance_data[0].msg.seqNum = instance_data[0].frame_sn++;
	
	//construct the frame header + function code
	DecawaveApp_SetupMacFrameData(&instance_data[0], RTLS_DEMO_MSG_TAG_POLL);
	
	#if (USING_64BIT_ADDR==1)
	instance_data[0].psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
	#else
	instance_data[0].psduLength = TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
	#endif
			
	//set the delayed rx on time (the response message will be sent after this delay)
	dwt_setrxaftertxdelay(instance_data[0].txToRxDelayTag_sy);
	dwt_setrxtimeout((uint16)instance_data[0].fwtoTime_sy);

	// write the frame data
	dwt_writetxdata(instance_data[0].psduLength, (uint8 *)  &instance_data[0].msg, 0) ;	

	dwt_writetxfctrl(instance_data[0].psduLength, 0);
	dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
}

/**@brief Function for sending a range init message
 *
 * @details Fill message with Tag 16 bits, resp anchor delay, resp tag delay and send it
 *
 */
void DecawaveApp_SendRangeInitMsg ()
{		
	uint16 resp_dly_us, resp_dly;
	
	if(instance_data[0].mode != ANCHOR) return;
									
	instance_data[0].psduLength = RANGINGINIT_MSG_LEN;

	//tell Tag what it's address will be for the ranging exchange
	instance_data[0].rng_initmsg.messageData[FCODE] = RTLS_DEMO_MSG_RNG_INIT;
	instance_data[0].rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_LO] = instance_data[0].tagShortAdd & 0xFF;
	instance_data[0].rng_initmsg.messageData[RNG_INIT_TAG_SHORT_ADDR_HI] = (instance_data[0].tagShortAdd >> 8) & 0xFF;

	// First response delay to send is anchor's response delay.
	resp_dly_us = ANC_TURN_AROUND_TIME_US + instance_data[0].fl_us[POLL];
	resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK) + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
	instance_data[0].rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_LO] = resp_dly & 0xFF;
	instance_data[0].rng_initmsg.messageData[RNG_INIT_ANC_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF;
                
	// Second response delay to send is tag's response delay.
	resp_dly_us = TAG_TURN_AROUND_TIME_US + instance_data[0].fl_us[RESP];
	resp_dly = ((RESP_DLY_UNIT_US << RESP_DLY_UNIT_SHIFT) & RESP_DLY_UNIT_MASK) + ((resp_dly_us << RESP_DLY_VAL_SHIFT) & RESP_DLY_VAL_MASK);
	instance_data[0].rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_LO] = resp_dly & 0xFF;
	instance_data[0].rng_initmsg.messageData[RNG_INIT_TAG_RESP_DLY_HI] = (resp_dly >> 8) & 0xFF;

	// write frame control
	instance_data[0].rng_initmsg.frameCtrl[0] = 0x41; 
	#if (USING_64BIT_ADDR == 1)
	instance_data[0].rng_initmsg.frameCtrl[1] = 0xCC;
	instance_data[0].psduLength += FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
	#else
	instance_data[0].rng_initmsg.frameCtrl[1] = 0x8C;
	instance_data[0].psduLength += FRAME_CRTL_AND_ADDRESS_LS + FRAME_CRC;
	#endif
		
	// write PAN ID
	instance_data[0].rng_initmsg.panID[0] = (instance_data[0].panid) & 0xff;
	instance_data[0].rng_initmsg.panID[1] = instance_data[0].panid >> 8;

	// write seq number
	instance_data[0].rng_initmsg.seqNum = instance_data[0].frame_sn++;

	// write the frame data
	dwt_writetxdata(instance_data[0].psduLength, (uint8 *)  &instance_data[0].rng_initmsg, 0) ;	

	//anchor - we don't use timeout, if the ACK is missed we'll get a Poll or Blink
	if(DecawaveApp_SendPacket(instance_data[0].psduLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, instance_data[0].delayedReplyTime))
	{
		// Error 
		dwt_setrxaftertxdelay(0);
		instance_data[0].lateTX++;
	}
	else
	{
		// Success
		//CONFIGURE FIXED PARTS OF RESPONSE MESSAGE FRAME (these won't change)
		//program option octet and parameters (not used currently)
		instance_data[0].msg.messageData[RES_R1] = 0x2; // "activity"
		instance_data[0].msg.messageData[RES_R2] = 0x0; //
		instance_data[0].msg.messageData[RES_R3] = 0x0;
							
		DecawaveApp_SetupMacFrameData(&instance_data[0], RTLS_DEMO_MSG_ANCH_RESP);	
		DecawaveApp_GoToRangingOperation();		
	}										
}

/**@brief Function for sending a response  message
 *
 * @details Fill message with Tag 16 bits, resp anchor delay, resp tag delay and send it
 *
 */
void DecawaveApp_SendRespMsg ()
{		
	if (instance_data[0].mode != ANCHOR) return;
	
	uint16 frameLength = 0;

	//remember who to send the reply to (set destination address)
	#if (USING_64BIT_ADDR == 1)
    frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
	#else
	frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
	#endif
						
	// Write calculated TOF into response message
	memcpy(&(instance_data[0].msg.messageData[TOFR]), &instance_data[0].tof, 5);
	instance_data[0].tof = 0; //clear ToF ..
	instance_data[0].msg.seqNum = instance_data[0].frame_sn++;

	//set the delayed rx on time (the final message will be sent after this delay)
	dwt_setrxaftertxdelay((uint32)instance_data[0].txToRxDelayAnc_sy);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)

	// write the frame data
	dwt_writetxfctrl(frameLength, 0);
	dwt_writetxdata(frameLength, (uint8 *)  &instance_data[0].msg, 0) ;	

	#if (IMMEDIATE_RESPONSE == 1)
	dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
	#else
	if(DecawaveApp_SendPacket(frameLength, DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED, inst->delayedReplyTime))
	{
		dwt_setrxaftertxdelay(0);
		inst->lateTX++;
	}
	else
	#endif	
}

/**@brief Function for sending a final  message
 *
 * @details Fill message with Tag 16 bits, resp anchor delay, resp tag delay and send it
 *
 */
void DecawaveApp_SendFinalMsg ()
{		
	if (instance_data[0].mode != TAG) return;
									
	// Prepare Final message
	// Write Function Code (0x29 for Final message at 1st octet)
	DecawaveApp_SetupMacFrameData(&instance_data[0], RTLS_DEMO_MSG_TAG_FINAL);

	// Write Response RX time field of Final message (position 6, size 5)	
	memcpy(&(instance_data[0].msg.messageData[RRXT]), (uint8 *)&instance_data[0].anchorRespRxTime, 5);

	// compute size of message
	#if (USING_64BIT_ADDR==1)
	instance_data[0].psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_L + FRAME_CRC;
	#else
	instance_data[0].psduLength = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
	#endif

	#if (DEEP_SLEEP == 1)
	if (instance_data[0].sleep_en)
	{
		dwt_entersleepaftertx(1);
	}
	#endif

	// write the frame data into tx buffer
	dwt_writetxdata(instance_data[0].psduLength, (uint8 *)  &instance_data[0].msg, 0) ;	
			
	// Send msg (delayed)
	if(DecawaveApp_SendPacket(instance_data[0].psduLength, DWT_START_TX_DELAYED, instance_data[0].delayedReplyTime))
	{
		// Transmit failed (too late)
		#if (DEEP_SLEEP == 1)
		if (instance_data[0].sleep_en)
		{
			dwt_entersleep();
		}
		#endif								
		instance_data[0].lateTX++;
		
		return;
	}
}


/**@brief Function for receiving range init msg
 *
 * @details 
 *
 * @param[in] messageData   range init message
 *
 */
void DecawaveApp_ReceiveRangeInitMsg (uint8* messageData)
{
	uint32 final_reply_delay_us;
	uint32 resp_dly[RESP_DLY_NB];
	int i;
	
	 instance_data[0].tagShortAdd = messageData[RNG_INIT_TAG_SHORT_ADDR_LO] + (messageData[RNG_INIT_TAG_SHORT_ADDR_HI] << 8) ;

     // Get response delays from message and update internal timings accordingly
     resp_dly[RESP_DLY_ANC] =  messageData[RNG_INIT_ANC_RESP_DLY_LO] + (messageData[RNG_INIT_ANC_RESP_DLY_HI] << 8);
     resp_dly[RESP_DLY_TAG] =  messageData[RNG_INIT_TAG_RESP_DLY_LO] + (messageData[RNG_INIT_TAG_RESP_DLY_HI] << 8);
     for (i = 0; i < RESP_DLY_NB; i++)
     {
         if (((resp_dly[i] & RESP_DLY_UNIT_MASK) >> RESP_DLY_UNIT_SHIFT) == RESP_DLY_UNIT_MS)
         {
			// Remove unit bit and convert to microseconds.
			resp_dly[i] &= ~RESP_DLY_UNIT_MASK;
			resp_dly[i] *= 1000;
		}
	}
	
	// Update delay between poll transmission and response reception.
    instance_data[0].txToRxDelayTag_sy = US_TO_SY_INT(resp_dly[RESP_DLY_ANC] -  instance_data[0].fl_us[POLL]) - RX_START_UP_SY;
						
    // Update delay between poll transmission and final transmission.
    final_reply_delay_us = resp_dly[RESP_DLY_ANC] + resp_dly[RESP_DLY_TAG];
    instance_data[0].finalReplyDelay = convertmicrosectodevicetimeu(final_reply_delay_us);
    instance_data[0].finalReplyDelay_ms = CEIL_DIV(final_reply_delay_us, 1000);			
	
	#if (DEEP_SLEEP == 1)
	if (instance_data[0].sleep_en)
	{
		//put device into low power mode
		dwt_entersleep(); //go to sleep
	}
	#endif	

	// Now that anchor is identified, we start transmition of POLL msg
    instance_data[0].mode = TAG ;
	DecawaveApp_GoToRangingOperation();
}

/**@brief Function for receiving final msg
 *
 * @param[in] messageData   final message
 * @param[in] tagFinalRxTime   timestamp 
 *
 */
void DecawaveApp_ReceiveFinalMsg	(uint8* messageData, uint64 tagFinalRxTime)
{
	int64 Rb, Da, Ra, Db ;
	double RaRbxDaDb = 0;
	double RbyDb = 0;
	double RayDa = 0;
	double distance = 0;

	uint64 tagPollTxTime  = 0;
	uint64 anchorRespRxTime  = 0;
	uint64 tagFinalTxTime  = 0;						
	
	instance_data[0].delayedReplyTime = 0 ;

    // times measured at Tag extracted from the message buffer
	// extract 40bit times
	memcpy (&tagPollTxTime, 	&(messageData[PTXT]), 5);
	memcpy (&anchorRespRxTime, 	&(messageData[RRXT]), 5);
	memcpy (&tagFinalTxTime, 	&(messageData[FTXT]), 5);

	// poll response round trip delay time is calculated as (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
	Ra = (int64)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT);
	Db = (int64)((instance_data[0].txu.anchorRespTxTime - instance_data[0].tagPollRxTime) & MASK_40BIT);

	// response final round trip delay time is calculated as (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
	Rb = (int64)((tagFinalRxTime - instance_data[0].txu.anchorRespTxTime) & MASK_40BIT);
	Da = (int64)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT);

	RaRbxDaDb = (((double)Ra))*(((double)Rb)) - (((double)Da))*(((double)Db));
	RbyDb = ((double)Rb + (double)Db);
	RayDa = ((double)Ra + (double)Da);
					   
	// time-of-flight
	int64 tof  = (int64) ( RaRbxDaDb /(RbyDb + RayDa) );
	distance = DecawaveApp_CalculateDistance (tof);

	dwt_setrxaftertxdelay(0);
	
	//this will update the antenna delay if it has changed	
	DecawaveApp_SetAntennaDelays(); 
						
	//turn RX on without delay		
	DecawaveApp_rxON(&instance_data[0], 0, 0) ;  
						
	// Inform the BLE service that  new data is ready to send	
	DecawaveApp_ReportDistanceMeasurement(distance);	
}


/**
 * @brief Function for waking the device up
 *
 */
void DecawaveApp_WakeUp ()
{
#if (DEEP_SLEEP == 1)
    if (instance_data[0].sleep_en)
    {
		uint32 x = 0;
                
		//wake up device from low power mode
		GPIO_SetPinValue (PIN_DECA_WAKEUP, 1);

        Sleep(6);   
		//200 us to wake up 
        //then wait 5ms for DW1000 XTAL to stabilise

		//need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
		//when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us)
		
		GPIO_SetPinValue (PIN_DECA_WAKEUP, 0);

		//!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state
		//do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs
		x = dwt_readdevid(); //dummy read... need to wait for 11 us to exit INIT state (5 SPI bytes @ ~8 MHz)
		x = dwt_readdevid(); //dummy read... need to wait for 11 us to exit INIT state (5 SPI bytes @ ~8 MHz)
		x = dwt_readdevid(); //dummy read... need to wait for 11 us to exit INIT state (5 SPI bytes @ ~8 MHz)
		x = dwt_readdevid(); //dummy read... need to wait for 11 us to exit INIT state (5 SPI bytes @ ~8 MHz)

		if(x != DWT_DEVICE_ID)
		{
			return;
		}

		//this is platform dependent - only program if DW EVK/EVB
//    	dwt_setleds(1);
		
		//MP bug - TX antenna delay needs reprogramming as it is not preserved after DEEP SLEEP
		dwt_settxantennadelay(instance_data[0].txantennaDelay) ;

		//set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
		dwt_entersleepaftertx(0);
		dwt_setinterrupt(DWT_INT_TFRS, 1); //re-enable the TX/RX interrupts
	}
		
	//this will update the antenna delay if it has changed	
	DecawaveApp_SetAntennaDelays(); 
#endif
}

/**
 * @brief Function for putting the device into sleep mode
 *
 */
void DecawaveApp_EnterSleep ()
{
#if (DEEP_SLEEP == 1)
    if (instance_data[0].sleep_en)
	{
		//put device into low power mode
		dwt_entersleep(); //go to sleep
	}
#endif	
}

void DecawaveApp_ReportDistanceMeasurement (double distance)
{
	(*ptr_ReportDistanceMeasurement)(distance);
}

void DecawaveApp_GoToRangingOperation ()
{
	(*ptr_GoToRangingOperation)();
}

