/**
 * @file app_uwbrange.c
 * @brief Implementation of uwbrange service (app)
 * @version 6.0
 * @date 27/06/2017
 */
 
// Standard includes
#include <stdint.h>
#include <string.h>

// Nordic includes
#include "nordic_common.h"
#include "nrf.h"
#include "app_error.h"
#include "app_timer.h"

// BLE Services
#include "ble_uwbrange.h"
#include "app_uwbrange.h"
#include "ble_bas.h"
 
 // DecawaveApp
#include "decawave_app.h"

// Platform
#include "interrupts.h"
#include "GPIO.h"
#include "adc.h"

// Misc
 #include "GlobalDefs.h"
 #include "boards.h"
 
//**************************************************************************************************
// STATIC VARS 
//*************************************************************************************************/
 
// Service identifiers
static ble_uwbrange_t 						m_uwbrange;  		/**< Structure used to identify the uwbrange service. */
static ble_bas_t 							m_bas;              /**< Structure used to identify the battery service. */

// Controls char.
#if CONFIG_AS_TAG
static uint8_t charac_controls = 			1;					/**< controls characteristic */
#else
static uint8_t charac_controls = 			0;					/**< controls characteristic */
#endif

 // AVG uwbrange vars
static uint8_t charac_avg_configuration = 	8;					/**< number of samples used to calculate the average distance. */
static uint8_t averageCounter =				0;					/**< counter used to calculate the average distance. */
static double totalDistance =				0;					/**< total used to calculate the average distance. */

// Events & Threshold uwbrange vars
#if CONFIG_AS_TAG
static uint8_t charac_status = 				1;					/**< Event Charac value */
#else
static uint8_t charac_status = 				0;					/**< Event Charac value */
#endif
static double lowThreshold =				100.0;				/**< low theshold, initated at 100 cm . */
static double highThreshold =				120.0;				/**< high theshold, initated at 120 cm . */
static uint8_t previousThresholdStates = 	0x0;				/**< Previous state, used to detect if a change is detected */
static bool firstThresholdRequest = 		true;				/**< used to determine the first threshold */
 
 // Timers vars
APP_TIMER_DEF (m_blink_timer_id);                            	/**< blink timer ID. */
APP_TIMER_DEF (m_poll_timer_id);                            	/**< poll timer ID. */
APP_TIMER_DEF (m_batt_timer_id);                            	/**< batt timer ID. */
 
//battery
static uint8_t  battery_level			= 100 ;

// Others
static bool serviceStarted = 				false;				
 
//***************************************************************************************************
// TIMERS
//
// We have 3 timers based on the RTC1 clock.
//
// - blink (TAG only) 	: defines how long we wait between each "blink" msg send
// - poll (TAG only)  	: defines how long we wait between each "poll" msg send
// - batt 			  	: defines delay between battery measurements
//
//*************************************************************************************************/	

/**@brief Start Poll timer (delay between each poll message)
 */
void start_poll_timer()
{
	uint32_t err_code;
	err_code = app_timer_start (m_poll_timer_id, APP_TIMER_TICKS(DecawaveApp_GetTagSleepDelay(), APP_TIMER_PRESCALER) , NULL);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Stop poll timer (delay between each poll message)
 */
void stop_poll_timer()
{
	uint32_t err_code;
	err_code = app_timer_stop (m_poll_timer_id);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Start blink timer (delay between each blink message)
 */
void start_blink_timer()
{
	uint32_t err_code;
	err_code = app_timer_start (m_blink_timer_id, DATA_UPDATE_1000_INTERVAL, NULL);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Stop blink timer (delay between each blink message)
 */
void stop_blink_timer()
{
	uint32_t err_code;
	err_code = app_timer_stop (m_blink_timer_id);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Start batt timer 
 */
void start_batt_timer()
{
	uint32_t err_code;
	err_code = app_timer_start (m_batt_timer_id, DATA_UPDATE_60000_INTERVAL, NULL);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Stop batt timer
 */
void stop_batt_timer()
{
	uint32_t err_code;
	err_code = app_timer_stop (m_batt_timer_id);
	APP_ERROR_CHECK (err_code);			
}

/**@brief Function for handling blink messages
 * @param p_context Pointer to Context
 */
void blink_timeout_handler(void * p_context)
{
    UNUSED_PARAMETER(p_context);
	
	// here, the decawave should be in sleep mode, wake it up !
	DecawaveApp_WakeUp ();
		
	// Send BLINK MSG and wait for a RANGE INIT MSG from ANCHOR
	DecawaveApp_SendBlinkMsg ();
}

/**@brief Function for handling poll messages
 * @param p_context Pointer to Context
 */
void poll_timeout_handler(void * p_context)
{
    UNUSED_PARAMETER(p_context);
	
	// here, the decawave should be in sleep mode, wake it up !
	DecawaveApp_WakeUp ();
		
	// Send POLL MSG
	DecawaveApp_SendPollMsg();
}

/**@brief Function for handling batt timeout
 * @param p_context Pointer to Context
 */
void batt_timeout_handler(void * p_context)
{
    UNUSED_PARAMETER(p_context);
	ADC_sample();
}

/**@brief Function for the Timer initialization.
 *
 * @details We create 3 timers:
 *				- Blink timer which manage time between each blink msg
 *				- Poll timer which manage time between each poll msg
 *				- Batt timer which manage time between each battery measurement
 */
uint32_t timers_init (void)
{
    uint32_t err_code;

    // Initialize timer module.
    APP_TIMER_INIT (APP_TIMER_PRESCALER, APP_TIMER_OP_QUEUE_SIZE, false);
	
	// Create uwbrange timer.
    err_code  = app_timer_create (&m_blink_timer_id, 	APP_TIMER_MODE_REPEATED, 	blink_timeout_handler);
	err_code |= app_timer_create (&m_poll_timer_id, 	APP_TIMER_MODE_REPEATED, 	poll_timeout_handler);
	err_code |= app_timer_create (&m_batt_timer_id, 	APP_TIMER_MODE_REPEATED, 	batt_timeout_handler);
	
	//start batt timer
	start_batt_timer();
	
	return err_code;
}
 

//**************************************************************************************************
// SERVICE 
//
// We have 1 custom service (uwbrange) with the following characteristics:
//			- uwbrange_inst_measurement 		: contains instant range measurement
//			- uwbrange_avg_measurement 			: contains mean range measurement
//			- uwbrange_period_configuration 	: set the delay between each poll message send
//			- uwbrange_channel_configuration 	: configure the UWB signal
//			- uwbrange_mode_configuration 		: Set role of the module : TAG or ANCHOR
//			- uwbrange_avg_configuration 		: configure the sample number needed for the mean range measurement 
//			- uwbrange_threshold_configuration	: Set short and long range threshold values
//			- uwbrange_events 					: show events
//			- uwbrange_controls 				: controls
//*************************************************************************************************/	
 
/**@brief Function for initializing the uwbrange service
 *
 * @details Set permissions and initial values for all characteristics
 *
 */
void uwbrange_service_init(chConfig_t chConfig)
{
	uint32_t       err_code;
	ble_uwbrange_init_t uwbrange_init_obj;
	
	memset(&uwbrange_init_obj, 0, sizeof(uwbrange_init_obj));
    
    uwbrange_init_obj.evt_handler          = on_uwbrange_evt;
    uwbrange_init_obj.support_notification = true;
    uwbrange_init_obj.p_report_ref         = NULL;

	// instant measure
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.uwbrange_inst_measurement_char_attr_md.cccd_write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.uwbrange_inst_measurement_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS	(&uwbrange_init_obj.uwbrange_inst_measurement_char_attr_md.write_perm);
	uwbrange_init_obj.uwbrange_inst_measurement_initial_value = 0;
	
	// avg measure
	BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.uwbrange_avg_measurement_char_attr_md.cccd_write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.uwbrange_avg_measurement_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS	(&uwbrange_init_obj.uwbrange_avg_measurement_char_attr_md.write_perm);
	uwbrange_init_obj.uwbrange_avg_measurement_initial_value = 0;
	
	// speed conf
	BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.period_configuration_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.period_configuration_char_attr_md.write_perm);
	uwbrange_init_obj.period_configuration_initial_value = 500;
	
	// Channel conf
	BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.channel_configuration_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.channel_configuration_char_attr_md.write_perm);	
	uint8 pl = 0;
	switch(chConfig.preambleLength)
	{
		case DWT_PLEN_4096:
			pl=0;
			break;
		case DWT_PLEN_2048:
			pl=1;
			break;
		case DWT_PLEN_1536:
			pl=2;
			break;
		case DWT_PLEN_1024:
			pl=3;
			break;
		case DWT_PLEN_512:
			pl=4;
			break;		
		case DWT_PLEN_256:
			pl=5;
			break;		
		case DWT_PLEN_128:
			pl=6;
			break;		
		case DWT_PLEN_64:
			pl=7;
			break;		
		default:
			pl=6;
			break;			
	}
	uwbrange_init_obj.channel_configuration_initial_value =    (((chConfig.prf-1) & DECA_CONF_PRF_SIZE) << DECA_CONF_PRF_OFFSET)
															 | ((chConfig.datarate & DECA_CONF_DR_SIZE) << DECA_CONF_DR_OFFSET)
															 | ((pl & DECA_CONF_PLEN_SIZE) << DECA_CONF_PLEN_OFFSET) 	
															 | ((chConfig.preambleCode  & DECA_CONF_PC_SIZE) << DECA_CONF_PC_OFFSET)
															 | ((chConfig.channel & DECA_CONF_CH_SIZE)	<< DECA_CONF_CH_OFFSET)
															 | ((chConfig.nsSFD & DECA_CONF_SFD_SIZE) << DECA_CONF_SFD_OFFSET);														 
															 
	
	// role conf
	BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.mode_configuration_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.mode_configuration_char_attr_md.write_perm);
	#if CONFIG_AS_TAG
	uwbrange_init_obj.mode_configuration_initial_value = 1;
	#else
	uwbrange_init_obj.mode_configuration_initial_value = 2;
	#endif
	
	// AVG conf
	BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.avg_configuration_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.avg_configuration_char_attr_md.write_perm);
	uwbrange_init_obj.avg_configuration_initial_value = charac_avg_configuration;
	
	// Threshold conf
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.threshold_configuration_char_attr_md.write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.threshold_configuration_char_attr_md.read_perm);
	uwbrange_init_obj.threshold_configuration_initial_value = 0x780064;
	lowThreshold = lowThreshold;												
	highThreshold =	highThreshold;	
	
	// status conf	
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.status_char_attr_md.cccd_write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.status_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS	(&uwbrange_init_obj.status_char_attr_md.write_perm);
	uwbrange_init_obj.status_initial_value = charac_status;
	
	// Controls conf
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.controls_char_attr_md.write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&uwbrange_init_obj.controls_char_attr_md.read_perm);
	uwbrange_init_obj.controls_initial_value = charac_controls;
    
	// create service
    err_code = ble_uwbrange_init(&m_uwbrange, &uwbrange_init_obj);
	APP_ERROR_CHECK(err_code);
}

/**@brief Set events from ble_uwbrange service
 */
void set_ble_uwbrange_on_ble_evt(ble_evt_t *p_ble_evt)
{
	ble_uwbrange_on_ble_evt (&m_uwbrange, p_ble_evt);
}

/**@brief Function for updating the Distance characteristic value
 *
 * @details Update the instant uwbrange characteristic and notify it
 *			Add value to the "average buffer" and if the "averageCounter" reaches the "charac_avg_configuration", update the average uwbrange characteristic and notify it
 * 			Update and notify events characteristic with new threshold values if necessary
 *
 * @param[in]	distance  value of the distance
 */
void uwbrange_measurement_update (double distance)
{
    uint32_t err_code;
	uint8_t thresholdStates = 0x0;
	char strTemp[50];
	
	// Notify Instant Distance Value via BLE
	err_code = ble_uwbrange_instant_measurement_update (&m_uwbrange, distance);
	if (
		(err_code != NRF_SUCCESS)
		&&
		(err_code != NRF_ERROR_INVALID_STATE)
		&&
		(err_code != BLE_ERROR_NO_TX_PACKETS)
		&&
		(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
		)
	{
		APP_ERROR_HANDLER (err_code);
	}
	
	// add Instant Distance Value to the total in order to calculate the average value
	totalDistance += distance;
	averageCounter++;
		
	// number of value reached, we divide the total by charac_avg_configuration and send result via BLE 
	if (averageCounter == charac_avg_configuration)
	{
		totalDistance = totalDistance / charac_avg_configuration;
		averageCounter = 0;
			
		// Notify Average uwbrange value via BLE
		err_code = ble_uwbrange_avg_measurement_update (&m_uwbrange, totalDistance);
		if (
			(err_code != NRF_SUCCESS)
			&&
			(err_code != NRF_ERROR_INVALID_STATE)
			&&
			(err_code != BLE_ERROR_NO_TX_PACKETS)
			&&
			(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
			)
		{
			APP_ERROR_HANDLER (err_code);
		}
		totalDistance = 0;
	}	
	
	sprintf(strTemp, "%f" , distance);
	sscanf(strTemp, "%lf", &distance);
		
	// check thresholds
	if (distance < lowThreshold/100) thresholdStates = 0x1;
	if (distance < highThreshold/100) thresholdStates |= 0x2;
	
	if (thresholdStates != previousThresholdStates || firstThresholdRequest)
	{	
		charac_status &= ~(0x1 << BLE_DIST_STATUS_STHRESHOLD);
		charac_status &= ~(0x1 << BLE_DIST_STATUS_LTHRESHOLD);
		charac_status |= (thresholdStates << 1);
		
		// Notify events via BLE
		err_code = ble_uwbrange_status_update (&m_uwbrange, charac_status);
		if (
			(err_code != NRF_SUCCESS)
			&&
			(err_code != NRF_ERROR_INVALID_STATE)
			&&
			(err_code != BLE_ERROR_NO_TX_PACKETS)
			&&
			(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
			)
		{
			APP_ERROR_HANDLER (err_code);
		}	
		
		previousThresholdStates = thresholdStates;
		firstThresholdRequest = false;
	}
}

/**@brief Function for handling the Distance service events.
 *
 * @param[in] p_uwbrange  Pointer to the uwbrange service identifier.
 * @param[in] p_evt  Pointer to the Distance service event.
 */
void on_uwbrange_evt(ble_uwbrange_t * p_uwbrange, ble_uwbrange_evt_t * p_evt)
{
	switch (p_evt->evt_type)
    {	
		case BLE_DIST_EVT_PERIOD_CHANGED:
			// Set Period	
			DecawaveApp_SetTagSleepDelay((p_evt->value), 500);
			break;
		
		case BLE_DIST_EVT_CONTROLS_CHANGED:
			// we start ranging operation
			if (p_evt->value & 0x01)
			{
				firstThresholdRequest = true;
				serviceStarted = true;
				
				// Prepare Decawave for exchange
				DecawaveApp_InitExchange();
				
				// Switch to discovery mode
				GoToDiscoveryMode();
			}
			// we stop ranging operation
			else
			{
				GoToIdleMode();

				if (serviceStarted) DecawaveApp_EndExchange();
				serviceStarted = false;

			}
			break;
		
		case BLE_DIST_EVT_UWB_CHANGED:
			DecawaveApp_ChannelConfigure (p_evt->value);
			break;
		
		case BLE_DIST_EVT_ROLE_CHANGED:
			DecawaveApp_SetRole (p_evt->value & 0x3);
			break;
		
		case BLE_DIST_EVT_THRESHOLD_CHANGED:
			lowThreshold = ((p_evt->value) & 0x7FFF);
			highThreshold = ((p_evt->value >> 16) & 0x7FFF);
			break;
		
		case BLE_DIST_EVT_AVG_CHANGED:
			charac_avg_configuration = p_evt->value;
			averageCounter = 0;
			totalDistance = 0;
			break;
		
        default:
            break;
    }
}

//**************************************************************************************************
// OTHERS
//*************************************************************************************************/	

/**@brief Function for going to Ranging mode
 *
 * @details Notify via BLE that we now are in ranging mode
 *			Start "poll" timer if indtance is TAG 
 */
void GoToRangingMode()
{
	uint32_t err_code;
	
	charac_status &= ~(0x1 << BLE_DIST_STATUS_DISCMODE);
	charac_status |= (0x1 << BLE_DIST_STATUS_RANGINGMODE);
		
	// Notify events via BLE
	err_code = ble_uwbrange_status_update (&m_uwbrange, charac_status);
	if (
		(err_code != NRF_SUCCESS)
		&&
		(err_code != NRF_ERROR_INVALID_STATE)
		&&
		(err_code != BLE_ERROR_NO_TX_PACKETS)
		&&
		(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
		)
	{
		APP_ERROR_HANDLER (err_code);
	}	
	
	if(instance_data[0].mode == TAG_TDOA || instance_data[0].mode == TAG)
	{
		start_poll_timer();
	}
}

/**@brief Function for going to Discovery mode
 *
 * @details Notify via BLE that we now are in discovery mode
 *			Start "blink" timer if indtance is TAG
 */
void GoToDiscoveryMode()
{
	uint32_t err_code;
	
	charac_status &= ~(0x1 << BLE_DIST_STATUS_RANGINGMODE);
	charac_status |= (0x1 << BLE_DIST_STATUS_DISCMODE);
		
	// Notify events via BLE
	err_code = ble_uwbrange_status_update (&m_uwbrange, charac_status);
	if (
		(err_code != NRF_SUCCESS)
		&&
		(err_code != NRF_ERROR_INVALID_STATE)
		&&
		(err_code != BLE_ERROR_NO_TX_PACKETS)
		&&
		(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
		)
	{
		APP_ERROR_HANDLER (err_code);
	}	
	
	if(instance_data[0].mode == TAG_TDOA || instance_data[0].mode == TAG)
	{
		//TAG => start blink timer
		start_blink_timer();
	}
}

/**@brief Function for going to IDLE mode
 *
 * @details Notify via BLE that we now are in IDLE mode
 *			Stop timers if indtance is TAG
 */
void GoToIdleMode()
{
	uint32_t err_code;
	
	charac_status &= ~(0x1 << BLE_DIST_STATUS_RANGINGMODE);
	charac_status &= ~(0x1 << BLE_DIST_STATUS_DISCMODE);
		
	// Notify events via BLE
	err_code = ble_uwbrange_status_update (&m_uwbrange, charac_status);
	if (
		(err_code != NRF_SUCCESS)
		&&
		(err_code != NRF_ERROR_INVALID_STATE)
		&&
		(err_code != BLE_ERROR_NO_TX_PACKETS)
		&&
		(err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
		)
	{
		APP_ERROR_HANDLER (err_code);
	}	
	
	
	if(instance_data[0].mode == TAG_TDOA || instance_data[0].mode == TAG)
	{
		//TAG => stop blink and poll timers
		stop_blink_timer();
		stop_poll_timer();
	}
}

/**@brief Set events from batt service
 */
void set_ble_bas_on_ble_evt(ble_evt_t *p_ble_evt)
{
	 ble_bas_on_ble_evt(&m_bas, p_ble_evt);
}

/**@brief Function for initializing the Battery Service.
 */
void bas_init(void)
{
	uint32_t       err_code;
    ble_bas_init_t bas_init_obj;	
	memset (&bas_init, 0, sizeof(bas_init_obj));

    // Here the sec level for the Battery Service can be changed/increased.
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&bas_init_obj.battery_level_char_attr_md.cccd_write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&bas_init_obj.battery_level_char_attr_md.read_perm);
    BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS	(&bas_init_obj.battery_level_char_attr_md.write_perm);
    BLE_GAP_CONN_SEC_MODE_SET_OPEN		(&bas_init_obj.battery_level_report_read_perm);

    bas_init_obj.evt_handler          = NULL;
    bas_init_obj.support_notification = true;
    bas_init_obj.p_report_ref         = NULL;
    bas_init_obj.initial_batt_level   = 100;

    err_code = ble_bas_init (&m_bas, &bas_init_obj);
    APP_ERROR_CHECK (err_code);
}

/**@brief Function for performing battery measurement and updating the Battery Level characteristic
 *        in Battery Service.
 */
 void battery_level_update(uint8_t value)
{
    uint32_t err_code;

    err_code = ble_bas_battery_level_update(&m_bas, value);
    if ((err_code != NRF_SUCCESS) &&
        (err_code != NRF_ERROR_INVALID_STATE) &&
        (err_code != BLE_ERROR_NO_TX_PACKETS) &&
        (err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)
       )
    {
        APP_ERROR_HANDLER(err_code);
    }
}
