/**
 * @file    decawave_app_common.c
 * @brief   DecaWave application level common 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"
 
instance_data_t instance_data[NUM_INST];

/**@brief Function for converting time to flight to distance
 *
 * @param[in] input_tof  time of flight
 */
double DecawaveApp_CalculateDistance (int64 input_tof)
{
	int64 tofi ;
	double distance ;
	double distance_to_correct ;
	double tof ;
	
	// check for negative results and accept them making them proper negative integers
	tofi = input_tof ;                          		// make it signed
	if (tofi > 0x007FFFFFFFFF)                  // MP counter is 40 bits,  close up TOF may be negative
	{
		tofi -= 0x010000000000 ;                // subtract fill 40 bit range to make it negative
	}	
	
	// convert to seconds (as floating point)
	tof = convertdevicetimetosec(tofi);          //this is divided by 4 to get single time of flight
	distance = tof * SPEED_OF_LIGHT;
	
#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(instance_data[0].configData.smartPowerEn)
	{
		//1.31 for channel 2 and 1.51 for channel 5
		if(instance_data[0].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(instance_data[0].configData.chan, (float) distance_to_correct, instance_data[0].configData.prf);
#endif

	if ((distance < 0) || (distance > 20000.000))    // discount any items with error
	{
		return 0;
	}
	else
		return distance;
}

/***************************************************************************************************
 * "SET" FUNCTIONS
 **************************************************************************************************/	

/** 
 *  @brief function to set the tag sleep time (in ms) 
 *
 *  @param[in] sleepdelay 		sleep delay ms
 *  @param[in] blinksleepdelay 	blink delay ms
 */
void DecawaveApp_SetTagSleepDelay(int sleepdelay, int blinksleepdelay) 
{
    instance_data[0].tagSleepTime_ms = sleepdelay ;
    instance_data[0].tagBlinkSleepTime_ms = blinksleepdelay ;
}

 /**@brief Set this instance role as the Tag, Anchor or Listener
 *
 * @param[in] inst_mode  mode
 */
void DecawaveApp_SetRole(int inst_mode)
{
    instance_data[0].mode =  (INST_MODE)inst_mode;               
}


/***************************************************************************************************
 *  "GET" FUNCTIONS
 **************************************************************************************************/	

/** 
 * @brief function to get the tag sleep time (in ms)
 *
 * @return tag sleep time in ms
 */
int DecawaveApp_GetTagSleepDelay() 
{
    return instance_data[0].tagSleepTime_ms;
}

/**
 * @brief Get this instance role
 *
 * @return mode
 */
int DecawaveApp_getrole(void)
{
    return instance_data[0].mode;
}

/**
 * @brief Get antenna delay
 *
 * @return antenna delay
 */
int DecawaveApp_get_dly(void)
{	
    int x = instance_data[0].txantennaDelay;

    return (x);
}

/** 
 *  @brief get blinkRXcount
 *
 *  @return blinkRXcount
 */
uint32 DecawaveApp_getblinkrxcount(void)
{
    return instance_data[0].blinkRXcount ;
}

/** 
 *  @brief get number of Rxed frames
 *
 *  @return rxmsgcount
 */
int DecawaveApp_get_rxf(void) 
{
    int x = instance_data[0].rxmsgcount;

    return (x);
}

/** 
 *  @brief get number of Txed frames
 *
 *  @return txmsgcount
 */
int DecawaveApp_get_txf(void)
{	
    int x = instance_data[0].txmsgcount;

    return (x);
}

/** 
 *  @brief get number of late Tx frames
 *
 *  @return lateTX
 */
int DecawaveApp_get_txl(void) 
{
    int x = instance_data[0].lateTX;

    return (x);
}

/** 
 *  @brief get number of late Rx frames
 *
 *  @return lateRX
 */
int DecawaveApp_get_rxl(void) 
{
    int x = instance_data[0].lateRX;

    return (x);
}

/** 
 *  @brief get tx antenna delay
 *
 *  @return tx antenna delay
 */
uint16 DecawaveApp_get_txantdly(void)
{
	return instance_data[0].txantennaDelay;
}

/** 
 *  @brief get rx antenna delay
 *
 *  @return rx antenna delay
 */
uint16 DecawaveApp_get_rxantdly(void)
{
	return instance_data[0].rxantennaDelay;
}

/***************************************************************************************************
 * OTHER INSTANCE FUNCTIONS
 **************************************************************************************************/	

/**
 * @brief function to select the destination address (e.g. the address of the next anchor to poll)
 *
 * @param[in] inst  	pointer to instance structure
 * @param[in] tagAddr  	pointer to tag address
 * @return 0
 */
int DecawaveApp_AddTagToList(instance_data_t *inst, uint8 *tagAddr)
{
    uint8 i;
    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};

    inst->blinkRXcount++ ;

    //add the new Tag to the list, if not already there and there is space
    for(i=0; i<TAG_LIST_SIZE; i++)
    {
        if(memcmp(&inst->tagList[i][0], &tagAddr[0], 8) != 0)
        {
            if(memcmp(&inst->tagList[i][0], &blank[0], 8) == 0) //blank entry
            {
                memcpy(&inst->tagList[i][0], &tagAddr[0], 8) ;
                inst->tagListLen = i + 1 ;
                break;
            }
        }
        else
        {
            break; //we already have this Tag in the list
        }
    }

    return 0;
}

/**
 * @brief function to clear tag list 
 *
 */
void DecawaveApp_ClearTagList(void)
{
    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};

    instance_data[0].tagListLen = 0 ;
    instance_data[0].blinkRXcount = 0 ;
    instance_data[0].tagToRangeWith = 0;

    memcpy(&instance_data[0].tagList[0][0], &blank[0], 8);
}

 /**@brief Clear all counts
 *
 */
void DecawaveApp_ClearCounts(void)
{
	dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP

    instance_data[0].rxTimeouts = 0;
    instance_data[0].frame_sn = 0;
    instance_data[0].frame_sn = 0;
    instance_data[0].txmsgcount = 0;
    instance_data[0].rxmsgcount = 0;
    instance_data[0].lateTX = 0;
    instance_data[0].lateRX = 0;

    DecawaveApp_ClearTagList();
} 

/**
 * @brief function to initialise instance structures
 *
 * @return Returns 0 on success and -1 on error
 */
int DecawaveApp_InitInstance(uint32_t DeviceNumber)
{
    int result;

    instance_data[0].mode = ANCHOR;                               
    instance_data[0].tof = 0;

    // Reset the IC (might be needed if not getting here from POWER ON)
    dwt_softreset();

	//we can enable any configuration loding from OTP/ROM on initialisation
    result = dwt_initialise(DWT_LOADUCODE | DWT_LOADLDOTUNE | DWT_LOADTXCONFIG | DWT_LOADANTDLY | DWT_LOADXTALTRIM) ;

    //this is platform dependant - 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
    }

	// Clear instance structurs
    DecawaveApp_ClearCounts() ;
	
    instance_data[0].sleep_en = 1;
    instance_data[0].panid = 0xdeca ;
	
	// get EUI
	dwt_geteui(instance_data[0].eui64);
	
	uint8   temp_eui64[8] = {DeviceNumber&0xFF, (DeviceNumber>>8)&0xFF, 0x00, 0x00, 0x00, 0x22, 0x22, 0x22};
	for (int i=0; i<8; i++)
		instance_data[0].eui64[i] = temp_eui64[i];
	
    return 0 ;
}

/** 
 *  @brief function to allow application configuration be passed into instance and affect underlying device opetation
 *
 *  @param[in] config pointer to UWB config structure
 */
void DecawaveApp_ConfigInstance(instanceConfig_t *config)
{
    int use_otpdata = DWT_LOADANTDLY | DWT_LOADXTALTRIM;
    uint32 power = 0;

    instance_data[0].configData.chan = config->channelNumber ;
    instance_data[0].configData.rxCode =  config->preambleCode ;
    instance_data[0].configData.txCode = config->preambleCode ;
    instance_data[0].configData.prf = config->pulseRepFreq ;
    instance_data[0].configData.dataRate = config->dataRate ;
    instance_data[0].configData.txPreambLength = config->preambleLen ;
    instance_data[0].configData.rxPAC = config->pacSize ;
    instance_data[0].configData.nsSFD = config->nsSFD ;
    instance_data[0].configData.phrMode = DWT_PHRMODE_STD ;
    instance_data[0].configData.sfdTO = config->sfdTO;

    //configure the channel parameters
    dwt_configure(&instance_data[0].configData, use_otpdata) ;

    instance_data[0].configTX.PGdly = txSpectrumConfig[config->channelNumber].PGdelay ;

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

	//if there are no calibrated values... need to use defaults
    if((power == 0x0) || (power == 0xFFFFFFFF)) 
    {
        power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M];
    }
    instance_data[0].configTX.power = power;

    //configure the tx spectrum parameters (power and PG delay)
    dwt_configuretxrf(&instance_data[0].configTX);

    instance_data[0].antennaDelayChanged = 0;

    //check if to use the antenna delay calibration values as read from the OTP
    if((use_otpdata & DWT_LOADANTDLY) == 0)
    {
    	instance_data[0].rxantennaDelay = instance_data[0].txantennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
        // -------------------------------------------------------------------------------------------------------------------
        // set the antenna delay, we assume that the RX is the same as TX.
        dwt_setrxantennadelay(instance_data[0].txantennaDelay);
        dwt_settxantennadelay(instance_data[0].txantennaDelay);
    }
    else
    {
        //get the antenna delay that was read from the OTP calibration area
    	instance_data[0].rxantennaDelay = instance_data[0].txantennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;

        // if nothing was actually programmed then set a reasonable value anyway
        if (instance_data[0].txantennaDelay == 0)
        {
        	instance_data[0].rxantennaDelay = instance_data[0].txantennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
            // -------------------------------------------------------------------------------------------------------------------
            // set the antenna delay, we assume that the RX is the same as TX.
            dwt_setrxantennadelay(instance_data[0].txantennaDelay);
            dwt_settxantennadelay(instance_data[0].txantennaDelay);
        }
    }

    if(config->preambleLen == DWT_PLEN_64) //if preamble length is 64
	{
		//reduce SPI to < 3MHz
    	SPI_SetLowRate(); 

		dwt_loadopsettabfromotp(0);

		 //increase SPI to max
		SPI_SetHighRate();
    }
}

/**
 * @brief Return the Device ID register value, enables higher level validation of physical device presence
 *
 * @return dwt_readdevid()
 */
uint32 DecawaveApp_ReadRevId(void)
{
    return dwt_readdevid() ;
}


/**@brief processrxtimeout
 */

void DecawaveApp_ProcessRxTimeout(instance_data_t *inst)
{
    inst->rxTimeouts ++ ;
	
    if(inst->mode == ANCHOR) 
    {
		//we did not receive the final/ACK - wait for next poll
		dwt_setrxtimeout(0);
    }
	else //if(inst->mode == TAG)
    {		
		#if (DEEP_SLEEP == 1)
		if (inst->sleep_en)
		{
			//put device into low power mode
			dwt_entersleep(); //go to sleep
		}
		#endif
    }

    //timeout - disable the radio (if using SW timeout the rx will not be off)
    dwt_forcetrxoff() ;
}

/**@brief function to initialise interrupts
 *
 * @param[in] 	mode  		TAG/ANCHOR?
 * @retval 		0       	succes
 * @retval 		-1       	error
 *
 */
int DecawaveApp_InitInterrupts(int mode)
{
    int instance = 0 ;

    instance_data[instance].mode =  (INST_MODE)mode;                               

    // 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);

    dwt_setcallbacks(DecawaveApp_TxCallback, DecawaveApp_RxCallback);

    return 0 ;
}


void DecawaveApp_ConfigAntennaDelays(uint16 tx, uint16 rx)
{	
	instance_data[0].txantennaDelay = tx ;
	instance_data[0].rxantennaDelay = rx ;

	instance_data[0].antennaDelayChanged = 1;
}

void DecawaveApp_SetAntennaDelays(void)
{
	if(instance_data[0].antennaDelayChanged == 1)
	{
		dwt_setrxantennadelay(instance_data[0].rxantennaDelay);
		dwt_settxantennadelay(instance_data[0].txantennaDelay);

		instance_data[0].antennaDelayChanged = 0;
	}
}


/**@brief function to construct the message/frame header bytes
 *
 * @details Fill panID and FrameControl
 *          
 */
void DecawaveApp_ConfigFrameHeader (instance_data_t *inst)
{
    inst->msg.panID[0] = (inst->panid) & 0xff;
    inst->msg.panID[1] = inst->panid >> 8;

    //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
    inst->msg.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
#if (USING_64BIT_ADDR==1)
    //source/dest addressing modes and frame version
    inst->msg.frameCtrl[1] = 0xC /*dest extended address (64bits)*/ | 0xC0 /*src extended address (64bits)*/;
#else
    inst->msg.frameCtrl[1] = 0x8 /*dest short address (16bits)*/ | 0x80 /*src short address (16bits)*/;
#endif
}


/**@brief function to configure the frame data, prior to writing the frame to the TX buffer
 *
 * @details Fill first octet of the user data part of the message (which corresponds to the function code)
 *          
 */
void DecawaveApp_SetupMacFrameData (instance_data_t *inst, int fcode)
{
    inst->msg.messageData[FCODE] = fcode; //message function code (specifies if message is a poll, response or other...)

	DecawaveApp_ConfigFrameHeader(inst);
}


/**@brief Turn on the receiver with/without delay
 *       
 */
void DecawaveApp_rxON(instance_data_t *inst, int delayed, uint64 delayedReceiveTime)
{
    if (delayed)
    {
        uint32 dtime;
        dtime =  (uint32) (delayedReceiveTime>>8);
        dwt_setdelayedtrxtime(dtime) ;
    }

	 // turn receiver on, immediate/delayed
    inst->lateRX -= dwt_rxenable(delayed) ;  //- as when fails -1 is returned            

}

/**@brief Send a delayed message
 *
 * @param[in] 	length  	Length of the message
 * @param[in] 	txmode  	mode of TX
 * @param[in] 	dtime  		timestamp from which data is sent
 * @retval 		0       	succes
 * @retval 		1       	late/error
 *
 */
int DecawaveApp_SendPacket(uint16 length, uint8 txmode, uint32 dtime)
{
    int result = 0;

    dwt_writetxfctrl(length, 0);
    if(txmode & DWT_START_TX_DELAYED)
    {
        dwt_setdelayedtrxtime(dtime) ;
    }

    //begin delayed TX of frame
    if (dwt_starttx(txmode))  
    {
        result = 1; //late/error
    }

    return result;       
}

extern uint8 dwnsSFDlen[];


/**@brief Pre-compute frame lengths, timeouts and delays needed in ranging process.
 *
 * @details /!\ This function assumes that there is no user payload in the frame.
 *
 */
void DecawaveApp_InitTimings(void)
{
    instance_data_t *inst = &instance_data[0];
    uint32 pre_len;
    int sfd_len;
    static const int data_len_bytes[FRAME_TYPE_NB] = {
        BLINK_FRAME_LEN_BYTES, RNG_INIT_FRAME_LEN_BYTES, POLL_FRAME_LEN_BYTES,
        RESP_FRAME_LEN_BYTES, FINAL_FRAME_LEN_BYTES};
    int i;
    // Margin used for timeouts computation.
    const int margin_sy = 100;//10; modif jf

    // All internal computations are done in tens of picoseconds before
    // conversion into microseconds in order to ensure that we keep the needed
    // precision while not having to use 64 bits variables.

    // Compute frame lengths.
    // First step is preamble plus SFD length.
    sfd_len = dwnsSFDlen[inst->configData.dataRate];
    switch (inst->configData.txPreambLength)
    {
		case DWT_PLEN_4096:
			pre_len = 4096;
			break;
		case DWT_PLEN_2048:
			pre_len = 2048;
			break;
		case DWT_PLEN_1536:
			pre_len = 1536;
			break;
		case DWT_PLEN_1024:
			pre_len = 1024;
			break;
		case DWT_PLEN_512:
			pre_len = 512;
			break;
		case DWT_PLEN_256:
			pre_len = 256;
			break;
		case DWT_PLEN_128:
			pre_len = 128;
			break;
		case DWT_PLEN_64:
			pre_len = 64;
			break;
    }
    pre_len += sfd_len;
	
    // Convert preamble length from symbols to time. Length of symbol is defined in IEEE 802.15.4 standard.
    if (inst->configData.prf == DWT_PRF_16M)
        pre_len *= 99359;
    else
        pre_len *= 101763;
	
    // Second step is data length for all frame types.
    for (i = 0; i < FRAME_TYPE_NB; i++)
    {
        // Compute the number of symbols for the given length.
        inst->fl_us[i] = data_len_bytes[i] * 8 + CEIL_DIV(data_len_bytes[i], 330) * 48;
		
        // Convert from symbols to time and add PHY header length.
        if(inst->configData.dataRate == DWT_BR_110K)
        {
            inst->fl_us[i] *= 820513;
            inst->fl_us[i] += 17230800;
        }
        else if (inst->configData.dataRate == DWT_BR_850K)
        {
            inst->fl_us[i] *= 102564;
            inst->fl_us[i] += 2153900;
        }
        else
        {
            inst->fl_us[i] *= 12821;
            inst->fl_us[i] += 2153900;
        }
        // Last step: add preamble length and convert to microseconds.
        inst->fl_us[i] += pre_len;
        inst->fl_us[i] = CEIL_DIV(inst->fl_us[i], 100000);
    }
	
    // Final frame wait timeout time.
    inst->fwtoTime_sy = US_TO_SY_INT(inst->fl_us[FINAL]) + RX_START_UP_SY + margin_sy;
	
    // Ranging init frame wait timeout time.
    inst->fwtoTimeB_sy = US_TO_SY_INT(inst->fl_us[RNG_INIT]) + RX_START_UP_SY + margin_sy;
	
    // Delay between blink transmission and ranging init reception.
    inst->rnginitW4Rdelay_sy = US_TO_SY_INT((RNG_INIT_REPLY_DLY_MS * 1000) - inst->fl_us[BLINK]) - RX_START_UP_SY;
	
    // Delay between anchor's response transmission and final reception.
    inst->txToRxDelayAnc_sy = US_TO_SY_INT(TAG_TURN_AROUND_TIME_US) - RX_START_UP_SY;

    // No need to init txToRxDelayTag_sy here as it will be set upon reception
    // of ranging init message.

    // Delay between blink reception and ranging init message transmission.
    inst->rnginitReplyDelay = convertmicrosectodevicetimeu(RNG_INIT_REPLY_DLY_MS * 1000);
	
    // Delay between poll reception and response transmission. Computed from
    // poll reception timestamp to response transmission timestamp so you have
    // to add poll frame length to delay that must be respected between frames.
	#if (IMMEDIATE_RESPONSE == 0)
    inst->responseReplyDelay = convertmicrosectodevicetimeu(ANC_TURN_AROUND_TIME_US + inst->fl_us[POLL]);
	#else
    inst->responseReplyDelay = 0 ;
	#endif

    // Smart Power is automatically applied by DW chip for frame of which length
    // is < 1 ms. Let the application know if it will be used depending on the
    // length of the longest frame.
    if (inst->fl_us[FINAL] <= 1000)
        inst->configData.smartPowerEn = 1;
    else
        inst->configData.smartPowerEn = 0;
}

/**@brief get own address
 * 
 * @retval 	64bits adress
 *
 */
uint64 DecawaveApp_get_addr(void) 
{
    int instance = 0;
    uint64 x = (uint64) instance_data[instance].eui64[0];
    x |= (uint64) instance_data[instance].eui64[1] << 8;
    x |= (uint64) instance_data[instance].eui64[2] << 16;
    x |= (uint64) instance_data[instance].eui64[3] << 24;
    x |= (uint64) instance_data[instance].eui64[4] << 32;
    x |= (uint64) instance_data[instance].eui64[5] << 40;
    x |= (uint64) instance_data[instance].eui64[6] << 48;
    x |= (uint64) instance_data[instance].eui64[7] << 56;
    return (x);
}

/**@brief get tag address
 * 
 * @retval 	64bits adress
 *
 */
uint64 DecawaveApp_get_tagaddr(void) 
{
    int instance = 0;
    uint64 x = (uint64) instance_data[instance].tagList[0][0];
    x |= (uint64) instance_data[instance].tagList[0][1] << 8;
    x |= (uint64) instance_data[instance].tagList[0][2] << 16;
    x |= (uint64) instance_data[instance].tagList[0][3] << 24;
    x |= (uint64) instance_data[instance].tagList[0][4] << 32;
    x |= (uint64) instance_data[instance].tagList[0][5] << 40;
    x |= (uint64) instance_data[instance].tagList[0][6] << 48;
    x |= (uint64) instance_data[instance].tagList[0][7] << 56;
    return (x);
}

/**@brief get anchor address (that sent the ToF)
 * 
 * @retval 	64bits adress
 *
 */
uint64 DecawaveApp_get_anchaddr(void) 
{
    int instance = 0;
    uint64 x = (uint64) instance_data[instance].relpyAddress[0];
    x |= (uint64) instance_data[instance].relpyAddress[1] << 8;
    x |= (uint64) instance_data[instance].relpyAddress[2] << 16;
    x |= (uint64) instance_data[instance].relpyAddress[3] << 24;
    x |= (uint64) instance_data[instance].relpyAddress[4] << 32;
    x |= (uint64) instance_data[instance].relpyAddress[5] << 40;
    x |= (uint64) instance_data[instance].relpyAddress[6] << 48;
    x |= (uint64) instance_data[instance].relpyAddress[7] << 56;
    return (x);
}
