/**
 * @file    decawave_app_callbacks.c
 * @brief   DecaWave application level callbacks instance functions
 * @version 6.0
 * @date 27/06/2017
 */
 
#include "deca_device_api.h"
#include "decawave_app.h"

// platform dependant includes
#include "sleep.h"
#include "spi.h"
#include "interrupts.h"


/**@brief called by dwt_isr() when a TX interrupt is detected
 *
 * @details 1)  Get TX timestamp
 *			2)  if msg is POLL : estimate TX FINAL timestamp prepare FINAL msg
 *				if msg is RESPONSE : save timepstamp and turn antenna ON
 *				if msg is RNG_INIT : turn antenna ON
 *
 * @param[in] txd  	pointer to dwt_callback_data_t
 */
void DecawaveApp_TxCallback(const dwt_callback_data_t *txd)
{
	int instance = 0;
	uint8 txTimeStamp[5] = {0, 0, 0, 0, 0};
	
	uint64 timeStamp ;			// timestamp
	uint32 timeStamp32l ;		// timestamp - low 32 bits
//	uint32 timeStamp32h ;		// timestamp - high 32 bits	
	
	uint8 txevent = txd->event;

	if(txevent == DWT_SIG_TX_DONE)
	{
		//NOTE - we can only get TX good (done) while here

		// Get TX Timestamp
		dwt_readtxtimestamp(txTimeStamp) ;
		timeStamp32l = (uint32)txTimeStamp[0] + ((uint32)txTimeStamp[1] << 8) + ((uint32)txTimeStamp[2] << 16) + ((uint32)txTimeStamp[3] << 24);
		timeStamp = txTimeStamp[4];
	    timeStamp <<= 32;
		timeStamp += timeStamp32l;
	//	timeStamp32h = ((uint32)txTimeStamp[4] << 24) + (timeStamp32l >> 8);
		
		// Read TX buffer 
		uint8   frame[STANDARD_FRAME_SIZE];
		dwt_readtxdata((uint8 *)&frame, txd->datalength, 0);  
		
		switch(frame[21])
		{
			// POLL MESSAGE SENT
			case RTLS_DEMO_MSG_TAG_POLL:
			{
				uint64 tagCalculatedFinalTxTime ;
				instance_data[instance].txu.txTimeStamp = timeStamp;
				
				// Embed into Final message: 40-bit pollTXTime,  40-bit respRxTime,  40-bit finalTxTime
				tagCalculatedFinalTxTime = (instance_data[instance].txu.txTimeStamp + instance_data[instance].finalReplyDelay) & MASK_TXDTS;  // time we should send the response
				instance_data[instance].delayedReplyTime = tagCalculatedFinalTxTime >> 8;

				// Calculate Time Final message will be sent and write this field of Final message
				// Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
				// zeroing its low 9 bits, and then having the TX antenna delay added
				// getting antenna delay from the device and add it to the Calculated TX Time
				tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + instance_data[instance].txantennaDelay;
				tagCalculatedFinalTxTime &= MASK_40BIT;

				// Write Calculated TX time field of Final message
				memcpy(&(instance_data[instance].msg.messageData[FTXT]), (uint8 *)&tagCalculatedFinalTxTime, 5);
				
				// Write Poll TX time field of Final message
				memcpy(&(instance_data[instance].msg.messageData[PTXT]), (uint8 *)&instance_data[instance].txu.tagPollTxTime, 5);
			}
			break;
		
			// RESPONSE MESSAGE SENT
			case RTLS_DEMO_MSG_ANCH_RESP:
			{
				instance_data[instance].txu.anchorRespTxTime = timeStamp;
				
				// turn RX on, with/without delay	
				DecawaveApp_rxON(&instance_data[instance], 0, 0) ;  
			}
			break;
			
			// RANGE INIT MESSAGE SENT
			case RTLS_DEMO_MSG_RNG_INIT:
			{
				// turn RX on, with/without delay	
				DecawaveApp_rxON(&instance_data[instance], 0, 0) ;   
			}	
			break;	
			
			default:
				break;
		}					

        instance_data[instance].txmsgcount++;
	}
}

/**@brief called by dwt_isr() when an RX interrupt is detected
 *
 * @details 1)  Get TX timestamp
 *			2)  if msg is BLINK : send RNG INIT msg
 *				if msg is RNG_INIT : get Response delay et set delay between poll and final msg
 *				if msg is POLL : send RESPONSE msg
 *				if msg is RESPONSE : send FINAL msg
 *				if msg is FINAL : get all timestamps and calculate TOF, turn antenna on and trigger SW interupt to alert the BLE service new data is avalaible
 *
 * @param[in] txd  	pointer to dwt_callback_data_t
 */
void DecawaveApp_RxCallback(const dwt_callback_data_t *rxd)
{
	int instance = 0;
	uint8 rxTimeStamp[5]  = {0, 0, 0, 0, 0};
	uint8 srcAddr_index = 0;
    uint8 rxd_event = 0;
	uint8 fcode_index  = 0;
	
	uint64 timeStamp ;			// timestamp
	uint32 timeStamp32l ;		// timestamp - low 32 bits
//	uint32 timeStamp32h ;		// timestamp - high 32 bits	
	
	union 
	{
		//holds received frame (after a good RX frame event)
		uint8   frame[STANDARD_FRAME_SIZE];
		srd_msg_dlsl rxmsg_ll ; //64 bit addresses
		srd_msg_dssl rxmsg_sl ;
		srd_msg_dlss rxmsg_ls ;
		srd_msg_dsss rxmsg_ss ; //16 bit addresses
		ack_msg rxackmsg ; //holds received ACK frame
		iso_IEEE_EUI64_blink_msg rxblinkmsg;
		iso_IEEE_EUI64_blinkdw_msg rxblinkmsgdw;
	}msgu;

	//if we got a frame with a good CRC - RX OK
    if(rxd->event == DWT_SIG_RX_OKAY)
	{
        rxd_event = DWT_SIG_RX_OKAY;

		//need to process the frame control bytes to figure out what type of frame we have received
        switch(rxd->fctrl[0])
	    {
			//blink type frame
		    case 0xC5:
				if(rxd->datalength == 12)
				{
					rxd_event = SIG_RX_BLINK;
			
				}
				//blink with Temperature and Battery level indication
				else if(rxd->datalength == 18)
				{
					rxd_event = SIG_RX_BLINK;
				}
				else
					rxd_event = SIG_RX_UNKNOWN;
					break;

			//ACK type frame - not supported in this SW - set as unknown (re-enable RX)
			case 0x02:
				rxd_event = SIG_RX_UNKNOWN;
			    break;

			//data type frames (with/without ACK request) - assume PIDC is on.
			case 0x41:
			case 0x61:
				//read the frame
				if(rxd->datalength > STANDARD_FRAME_SIZE)
					rxd_event = SIG_RX_UNKNOWN;

				//need to check the destination/source address mode
				if((rxd->fctrl[1] & 0xCC) == 0x88) //dest & src short (16 bits)
				{
					fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
					srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
				}
				else if((rxd->fctrl[1] & 0xCC) == 0xCC) //dest & src long (64 bits)
				{
					fcode_index = FRAME_CRTL_AND_ADDRESS_L; //function code is in first byte after source address
					srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L;
				}
				else //using one short/one long
				{
					fcode_index = FRAME_CRTL_AND_ADDRESS_LS; //function code is in first byte after source address

					if(((rxd->fctrl[1] & 0xCC) == 0x8C)) //source short
					{
						srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L;
					}
					else
					{
						srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
					}
				}
				break;

			//any other frame types are not supported by this application
			default:
				rxd_event = SIG_RX_UNKNOWN;
				break;
		}
	
		// Read data and get receive timestamp
		if((rxd_event == SIG_RX_BLINK) || (rxd_event == DWT_SIG_RX_OKAY))
		{
			//read rx timestamp
			dwt_readrxtimestamp(rxTimeStamp) ;
			timeStamp32l =  (uint32)rxTimeStamp[0] + ((uint32)rxTimeStamp[1] << 8) + ((uint32)rxTimeStamp[2] << 16) + ((uint32)rxTimeStamp[3] << 24);
			timeStamp = rxTimeStamp[4];
			timeStamp <<= 32;
			timeStamp += timeStamp32l;
	//		timeStamp32h = ((uint32)rxTimeStamp[4] << 24) + (timeStamp32l >> 8);

			// Read Data Frame
			dwt_readrxdata((uint8 *)&msgu.frame[0], rxd->datalength, 0);  
		}

		// We receive a BLINK MSG from TAG, process it
		if(rxd_event == SIG_RX_BLINK)
		{
			//add this Tag to the list of Tags we know about
			DecawaveApp_AddTagToList(&instance_data[instance], &(msgu.rxblinkmsg.tagID[0]));

			//initiate ranging message
			if(instance_data[instance].tagToRangeWith < TAG_LIST_SIZE)
			{
				if (!instance_data[instance].frameFilteringEnabled)
				{
					// if we missed the ACK to the ranging init message we may not have turned frame filtering on
					dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //we are starting ranging - enable the filter....
					instance_data[instance].frameFilteringEnabled = 1 ;
				}
				
				//initiate ranging message this is a Blink from the Tag we would like to range to
			
				if(memcmp(&instance_data[instance].tagList[instance_data[instance].tagToRangeWith][0],  &(msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS) == 0)
				{
					instance_data[instance].tagShortAdd = (dwt_getpartid() & 0xFF);
					instance_data[instance].tagShortAdd = (instance_data[instance].tagShortAdd << 8) + msgu.rxblinkmsg.tagID[0] ;

					//if using longer reply delay time (e.g. if interworking with a PC application)
					instance_data[instance].delayedReplyTime = (timeStamp + instance_data[instance].rnginitReplyDelay) >> 8 ;  // time we should send the blink response

					//set destination address
					memcpy(&instance_data[instance].rng_initmsg.destAddr[0], &(msgu.rxblinkmsg.tagID[0]), BLINK_FRAME_SOURCE_ADDRESS); //remember who to send the reply to
				}
			}
			
			// Prepare and send RANGE INIT Response to TAG
			DecawaveApp_SendRangeInitMsg ();						
		}	
		
		//Process other good frames
		if(rxd_event == DWT_SIG_RX_OKAY)
		{		
			//check if this is a TWR message (and also which one)
			if(instance_data[instance].tagListLen > 0 || instance_data[instance].mode == TAG || instance_data[instance].mode == TAG_TDOA)
			{
				switch(msgu.frame[fcode_index])
				{		
					// We receive an RNG INIT MSG from ANCHOR, process it				
					case RTLS_DEMO_MSG_RNG_INIT:
					{
						// anchor should not receive RNG_INIT msg - turn the antenna ON again
						if (instance_data[instance].mode == ANCHOR)
						{
							DecawaveApp_rxON(&instance_data[instance], 0, 0);
							break;
						}
							
						uint8 *messageData;
						uint8  srcAddr[8] = {0,0,0,0,0,0,0,0};
		
						// Get Anchor address
						memcpy(&srcAddr[0], &(msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L);
						
						// Read data
						messageData = &msgu.rxmsg_ll.messageData[0];	

						//set the anchor address for the reply (set destination address)
						#if (USING_64BIT_ADDR == 1)
						memcpy(&instance_data[instance].msg.destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_L); 
						#else
						memcpy(&instance_data[instance].msg.destAddr[0], &srcAddr[0], ADDR_BYTE_SIZE_S); 
						instance_data[instance].msg.sourceAddr[0] =  messageData[RES_R1]; //set tag short address
						instance_data[instance].msg.sourceAddr[1] =  messageData[RES_R2];
						dwt_setaddress16(instance_data[instance].tagShortAdd);
						#endif	
	
						//remember who to send the reply to (set destination address)
						memcpy(&instance_data[instance].relpyAddress[0], &srcAddr[0], ADDR_BYTE_SIZE_L); 
										
						// analyse message data and take action
						DecawaveApp_ReceiveRangeInitMsg (messageData);
					}
					
					// We receive a POLL MSG from TAG, process it
					case RTLS_DEMO_MSG_TAG_POLL:
					{		
						// Save Poll's Rx time
						instance_data[instance].tagPollRxTime = timeStamp ; 
						
						// time we should send the response
						#if (IMMEDIATE_RESPONSE == 0)
						instance_data[instance].delayedReplyTime = (instance_data[instance].tagPollRxTime + inst->responseReplyDelay) >> 8 ;  
						#else
						instance_data[instance].delayedReplyTime = 0;
						#endif
						
						//remember who to send the reply to (set destination address)
						#if (USING_64BIT_ADDR == 1)
						memcpy(&instance_data[instance].msg.destAddr[0], &msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_L); 
						#else
						memcpy(&instance_data[instance].msg.destAddr[0], &msgu.frame[srcAddr_index], ADDR_BYTE_SIZE_S); 
						#endif
						
						// Prepare and send ANCHOR RESP msg to TAG						
						DecawaveApp_SendRespMsg ();
					}
					break;

					// We receive a RESPONSE from ANCHOR, process it
					case RTLS_DEMO_MSG_ANCH_RESP:
					{
						// anchor should not receive RESPONSE msg - turn the antenna ON again
						if (instance_data[instance].mode == ANCHOR)
						{
							DecawaveApp_rxON(&instance_data[instance], 0, 0);
							break;
						}
						
						// Get destination address
						uint8  dstAddr[8] = {0,0,0,0,0,0,0,0};
						memcpy(&dstAddr[0], &(msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L);
						
						if(memcmp(&instance_data[instance].eui64, dstAddr, 8))
						{
							break;
						}
						
						// Get source address
						uint8  srcAddr[8] = {0,0,0,0,0,0,0,0};
						memcpy(&srcAddr[0], &(msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L);
						
						// Save Response's Rx time				
						instance_data[instance].anchorRespRxTime = timeStamp ;
						
						// Get message
						uint8 *messageData;		
						messageData = &msgu.rxmsg_ll.messageData[0];
						
						// copy previously calculated ToF
						instance_data[instance].tof = 0;
						memcpy(&instance_data[instance].tof, &(messageData[TOFR]), 5);

						instance_data[instance].newrangeancaddress = srcAddr[0] + ((uint16) srcAddr[1] << 8);
						instance_data[instance].newrangetagaddress = instance_data[instance].eui64[0] + ((uint16) instance_data[instance].eui64[1] << 8);
						
						// Prepare and send FINAL msg to ANCHOR	
						DecawaveApp_SendFinalMsg ();
					}
					break;
					
					//  We receive a FINAL MSG from TAG, process it
					case RTLS_DEMO_MSG_TAG_FINAL:
					{
						uint8  srcAddr[8] = {0,0,0,0,0,0,0,0};
						uint8 *messageData;
						uint64 tagFinalRxTime  = 0;
						
						// time of arrival of Final message
						tagFinalRxTime = timeStamp ; //Final's Rx time
						
						memcpy(&srcAddr[0], &(msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L);
						messageData = &msgu.rxmsg_ll.messageData[0];

						instance_data[instance].newrangetagaddress = srcAddr[0] + ((uint16) srcAddr[1] << 8);
						instance_data[instance].newrangeancaddress = instance_data[instance].eui64[0] + ((uint16) instance_data[instance].eui64[1] << 8);
						
						// analyse message data and take action
						DecawaveApp_ReceiveFinalMsg (messageData, tagFinalRxTime);
					}
					break;
					
					default: //process rx frame
						break;
				}
			}
			else if (instance_data[instance].mode == ANCHOR)
			{
				DecawaveApp_rxON(&instance_data[instance], 0, 0); //immediate enable
			}

            instance_data[instance].rxmsgcount++;
		}
		else if (rxd_event == SIG_RX_BLINK)
		{
            instance_data[instance].rxmsgcount++;
		}

		if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx
		{
			DecawaveApp_rxON(&instance_data[instance], 0, 0); //immediate enable
		}
	}
	else if (rxd->event == DWT_SIG_RX_TIMEOUT)
	{
		timeStamp = 0;
		timeStamp32l = 0;
//		timeStamp32h = 0;

		DecawaveApp_ProcessRxTimeout(&instance_data[instance]);
	}
	else //assume other events are errors
	{
		//re-enable the receiver
		//for ranging application rx error frame is same as TO - as we are not going to get the expected frame
		if((instance_data[instance].mode == TAG) || (instance_data[instance].mode == TAG_TDOA))
		{	
			#if (DEEP_SLEEP == 1)
			if (instance_data[instance].sleep_en)
			{
				//put device into low power mode
				dwt_entersleep(); //go to sleep
			}
			#endif	
		}
		else
		{
			DecawaveApp_rxON(&instance_data[instance], 0, 0); //immediate enable if anchor or listener
		}
	}
}
