/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
 *
 * The information contained herein is property of Nordic Semiconductor ASA.
 * Terms and conditions of usage are described in detail in NORDIC
 * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
 *
 * Licensees are granted free, non-transferable use of the information. NO
 * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
 * the file.
 *
 */

/**
 * @file ble_uwbRange.c
 * @brief Impmentation of uwbrange service (ble)				
 * @version 6.0
 * @date 27/06/2017
 */
 
#include "ble_uwbrange.h"
#include "ble_srv_common.h"
#include "sdk_common.h"

/**@brief function handling a "connect" event 
 *
 * @param[in]	p_uwbrange  pointer to the service structure
 * @param[in]	p_ble_evt   pointer to the BLE Event type struct
 */
void on_uwbrange_connect (ble_uwbrange_t * p_uwbrange, ble_evt_t * p_ble_evt)
{
    p_uwbrange->conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
}

/**@brief function handling a "disconnect" event 
 *
 * @param[in]	p_uwbrange 	pointer to the service structure
 * @param[in]	p_ble_evt   pointer to the BLE Event type struct
 */
void on_uwbrange_disconnect (ble_uwbrange_t * p_uwbrange, ble_evt_t * p_ble_evt)
{
    UNUSED_PARAMETER(p_ble_evt);
    p_uwbrange->conn_handle = BLE_CONN_HANDLE_INVALID;
}

/**@brief function handling a "write" event 
 *
 * @param[in]	p_uwbrange 	pointer to the service structure
 * @param[in]	p_ble_evt   pointer to the BLE Event type struct
 */
void on_uwbrange_write (ble_uwbrange_t *p_uwbrange, ble_evt_t *p_ble_evt)
{
	ble_gatts_evt_write_t * p_evt_write = &p_ble_evt->evt.gatts_evt.params.write;
	ble_uwbrange_evt_t evt;
	uint16_t temp, temp1;

	if (p_evt_write->handle == p_uwbrange->period_configuration_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_PERIOD_CHANGED;
		temp = p_evt_write->data[0];
		temp1 = p_evt_write->data[1];
		temp1 = temp1 << 8;
		evt.value = temp | temp1;
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
	else if (p_evt_write->handle == p_uwbrange->uwb_configuration_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_UWB_CHANGED;
		temp = p_evt_write->data[0];
		temp1 = p_evt_write->data[1];
		temp1 = temp1 << 8;
		evt.value = temp | temp1;
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
	else if (p_evt_write->handle == p_uwbrange->role_configuration_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_ROLE_CHANGED;
		evt.value = p_evt_write->data[0];
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
	else if (p_evt_write->handle == p_uwbrange->avg_configuration_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_AVG_CHANGED;
		evt.value = p_evt_write->data[0];
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
	else if (p_evt_write->handle == p_uwbrange->threshold_configuration_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_THRESHOLD_CHANGED;
		evt.value = p_evt_write->data[0] | (p_evt_write->data[1] << 8) | (p_evt_write->data[2] << 16) | (p_evt_write->data[3] << 24);
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
	else if (p_evt_write->handle == p_uwbrange->controls_handles.value_handle)
    {
		evt.evt_type = BLE_DIST_EVT_CONTROLS_CHANGED;
		evt.value = p_evt_write->data[0];
		p_uwbrange->evt_handler (p_uwbrange, &evt);
	}
}

/**@brief function handling uwbrange service events
 *
 * @details call specific event functions depending on the Event type ID
 *
 * @param[in]	p_uwbrange 	pointer to the service structure
 * @param[in]	p_ble_evt   pointer to the BLE Event type struct
 */
void ble_uwbrange_on_ble_evt (ble_uwbrange_t * p_uwbrange, ble_evt_t * p_ble_evt)
{
	switch (p_ble_evt->header.evt_id)
    {
        case BLE_GAP_EVT_CONNECTED:
            on_uwbrange_connect(p_uwbrange, p_ble_evt);
            break;
            
        case BLE_GAP_EVT_DISCONNECTED:
            on_uwbrange_disconnect(p_uwbrange, p_ble_evt);
            break;
            
        case BLE_GATTS_EVT_WRITE:
            on_uwbrange_write(p_uwbrange, p_ble_evt);
            break;           
        default:
            break;
    }
}

/**@brief Add uwbrange_inst_mesurement characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t uwbrange_inst_measurement_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
    
	// enable/disable notifications
    if (p_uwbrange->is_notification_supported)
    {
        memset(&cccd_md, 0, sizeof(cccd_md));   
        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
        cccd_md.write_perm = p_uwbrange_init->uwbrange_inst_measurement_char_attr_md.cccd_write_perm;
        cccd_md.vloc = BLE_GATTS_VLOC_STACK;
    }
    
	// Set properties
    memset(&char_md, 0, sizeof(char_md));
    char_md.char_props.read   = 1;
    char_md.char_props.notify = (p_uwbrange->is_notification_supported) ? 1 : 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = (p_uwbrange->is_notification_supported) ? &cccd_md : NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_INST_MEASUREMENT_CHAR;	
   
	// Set permissions
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->uwbrange_inst_measurement_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->uwbrange_inst_measurement_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

	// Add Characteristic to the service    
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(double);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(double);	 
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->uwbrange_inst_measurement_initial_value);
    
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->uwbrange_inst_measurement_handles);
	VERIFY_SUCCESS(err_code);
		
    return NRF_SUCCESS;
}

/**@brief Add uwbrange_avg_measurement characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t uwbrange_avg_measurement_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
    
	// enable/disable notifications
    if (p_uwbrange->is_notification_supported)
    {
        memset(&cccd_md, 0, sizeof(cccd_md));
        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
        cccd_md.write_perm = p_uwbrange_init->uwbrange_avg_measurement_char_attr_md.cccd_write_perm;
        cccd_md.vloc = BLE_GATTS_VLOC_STACK;
    }
 
	// Set properties	
    memset(&char_md, 0, sizeof(char_md)); 
    char_md.char_props.read   = 1;
    char_md.char_props.notify = (p_uwbrange->is_notification_supported) ? 1 : 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = (p_uwbrange->is_notification_supported) ? &cccd_md : NULL;
    char_md.p_sccd_md         = NULL; 
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_AVG_MEASUREMENT_CHAR;	
    
	// Set permissions
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->uwbrange_avg_measurement_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->uwbrange_avg_measurement_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

 	// Add Characteristic to the service           
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(double);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(double);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->uwbrange_avg_measurement_initial_value);
		
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->uwbrange_avg_measurement_handles);
	VERIFY_SUCCESS(err_code);
	    
    return NRF_SUCCESS;
}

/**@brief Add period_configuration characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t period_configuration_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;

	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
   
	// Set properties	
    memset(&char_md, 0, sizeof(char_md)); 
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_PERIOD_CONFIGURATION_CHAR;	
  
	// Set permissions  
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->period_configuration_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->period_configuration_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

  	// Add Characteristic to the service             
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint16_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint16_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->period_configuration_initial_value);
		
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->period_configuration_handles);
	VERIFY_SUCCESS(err_code);
    
    return NRF_SUCCESS;
}

/**@brief Add channel_configuration characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t uwb_configuration_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;

	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
  
	// Set properties		
    memset(&char_md, 0, sizeof(char_md));    
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_UWB_CONFIGURATION_CHAR;	
 
	// Set permissions   
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->channel_configuration_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->channel_configuration_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

   	// Add Characteristic to the service            
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint16_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint16_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->channel_configuration_initial_value);
    
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->uwb_configuration_handles);
	VERIFY_SUCCESS(err_code);

    return NRF_SUCCESS;
}

/**@brief Add avg_configuration characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t avg_configuration_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;

	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
  
	// Set properties		
    memset(&char_md, 0, sizeof(char_md));    
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_AVG_CONFIGURATION_CHAR;	
 
	// Set permissions   
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->avg_configuration_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->avg_configuration_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

   	// Add Characteristic to the service            
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint8_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint8_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->avg_configuration_initial_value);
    
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->avg_configuration_handles);
	VERIFY_SUCCESS(err_code);

    return NRF_SUCCESS;
}

/**@brief Add threshold characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t threshold_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;
//	ble_gatts_attr_md_t cccd_md;

	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
	
	     
	// Set properties		
    memset(&char_md, 0, sizeof(char_md));    
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_THRESHOLD_CONFIGURATION_CHAR;	
 
	// Set permissions   
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->threshold_configuration_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->threshold_configuration_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

   	// Add Characteristic to the service            
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint32_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint32_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->threshold_configuration_initial_value);
    
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->threshold_configuration_handles);
	VERIFY_SUCCESS(err_code);

    return NRF_SUCCESS;
}

/**@brief Add mode_configuration characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t role_configuration_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;

	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
  
	// Set properties			
    memset(&char_md, 0, sizeof(char_md)); 
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = NULL;
    char_md.p_sccd_md         = NULL;
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_ROLE_CONFIGURATION_CHAR;	
  
	// Set permissions     
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->mode_configuration_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->mode_configuration_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

    // Add Characteristic to the service              
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint8_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint8_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->mode_configuration_initial_value);
		
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->role_configuration_handles);
	VERIFY_SUCCESS(err_code);

    return NRF_SUCCESS;
}

/**@brief Add uwbrange_events characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t uwbrange_status_char_add(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
    
	// enable/disable notifications
    if (p_uwbrange->is_notification_supported)
    {
        memset(&cccd_md, 0, sizeof(cccd_md));
        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
        cccd_md.write_perm = p_uwbrange_init->status_char_attr_md.cccd_write_perm;
        cccd_md.vloc = BLE_GATTS_VLOC_STACK;
    }
 
	// Set properties	
    memset(&char_md, 0, sizeof(char_md)); 
    char_md.char_props.read   = 1;
    char_md.char_props.notify = (p_uwbrange->is_notification_supported) ? 1 : 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = (p_uwbrange->is_notification_supported) ? &cccd_md : NULL;
    char_md.p_sccd_md         = NULL; 
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_STATUS_CHAR;	
    
	// Set permissions
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->status_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->status_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

 	// Add Characteristic to the service           
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint8_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint8_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->status_initial_value);
		
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->status_handles);
	VERIFY_SUCCESS(err_code);
		
    return NRF_SUCCESS;
}

/**@brief Add uwbrange_controls characteristic to the service
 *
 * @param[in]	p_uwbrange 			pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 * @return  	err_code
 */
static uint32_t uwbrange_controls_char_add (ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t            err_code;
    ble_gatts_char_md_t char_md;
    ble_gatts_attr_md_t cccd_md;
    ble_gatts_attr_t    attr_char_value;
    ble_uuid_t          ble_uuid;
    ble_gatts_attr_md_t attr_md;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);
 
	// Set properties	
    memset(&char_md, 0, sizeof(char_md)); 
    char_md.char_props.read   = 1;
	char_md.char_props.write  = 1;
    char_md.char_props.notify = (p_uwbrange->is_notification_supported) ? 1 : 0;
    char_md.p_char_user_desc  = NULL;
    char_md.p_char_pf         = NULL;
    char_md.p_user_desc_md    = NULL;
    char_md.p_cccd_md         = (p_uwbrange->is_notification_supported) ? &cccd_md : NULL;
    char_md.p_sccd_md         = NULL; 
    
	// Set UUID
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_CONTROLS_CHAR;	
    
	// Set permissions
    memset(&attr_md, 0, sizeof(attr_md));
    attr_md.read_perm  = p_uwbrange_init->controls_char_attr_md.read_perm;
    attr_md.write_perm = p_uwbrange_init->controls_char_attr_md.write_perm;
    attr_md.vloc       = BLE_GATTS_VLOC_STACK;
    attr_md.rd_auth    = 0;
    attr_md.wr_auth    = 0;
    attr_md.vlen       = 0;

 	// Add Characteristic to the service           
    memset(&attr_char_value, 0, sizeof(attr_char_value));
    attr_char_value.p_uuid       = &ble_uuid;
    attr_char_value.p_attr_md    = &attr_md;
    attr_char_value.init_len     = sizeof(uint8_t);
    attr_char_value.init_offs    = 0;
    attr_char_value.max_len      = sizeof(uint8_t);
	attr_char_value.p_value   	 = (uint8_t*)&(p_uwbrange_init->controls_initial_value);
		
    err_code = sd_ble_gatts_characteristic_add(p_uwbrange->service_handle,
                                           &char_md,
                                           &attr_char_value,
                                           &p_uwbrange->controls_handles);
	VERIFY_SUCCESS(err_code);
		
    return NRF_SUCCESS;
}

/**@brief Initialize the uwbrange service
 *
 * @param[in]	p_uwbrange   		pointer to the service structure
 * @param[in]	p_uwbrange_init   	pointer to the init structure
 */
uint32_t ble_uwbrange_init(ble_uwbrange_t * p_uwbrange, const ble_uwbrange_init_t * p_uwbrange_init)
{
    uint32_t   err_code;
    ble_uuid_t ble_uuid;
	ble_uuid128_t   cs_base_uuid = LBS_UUID_BASE;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
    VERIFY_PARAM_NOT_NULL(p_uwbrange_init);

    // Initialize service structure
    //if (p_acc_init->evt_handler == NULL)
    p_uwbrange->evt_handler               = p_uwbrange_init->evt_handler;
    p_uwbrange->conn_handle               = BLE_CONN_HANDLE_INVALID;
    p_uwbrange->is_notification_supported = p_uwbrange_init->support_notification;
    
	// Add custom base UUID.
    err_code = sd_ble_uuid_vs_add (&cs_base_uuid, &p_uwbrange->uuid_type);
	VERIFY_SUCCESS(err_code);

	// Set UUID	
	ble_uuid.type = p_uwbrange->uuid_type;
    ble_uuid.uuid = BLE_UUID_UWBRANGE_SERVICE;
	
	// Add service
    err_code = sd_ble_gatts_service_add(BLE_GATTS_SRVC_TYPE_PRIMARY, &ble_uuid, &p_uwbrange->service_handle);
    VERIFY_SUCCESS(err_code);
    
    // Add characteristics
    err_code  = uwbrange_inst_measurement_char_add	(p_uwbrange, p_uwbrange_init);
    err_code |= uwbrange_avg_measurement_char_add	(p_uwbrange, p_uwbrange_init);
	err_code |= period_configuration_char_add		(p_uwbrange, p_uwbrange_init);
	err_code |= uwb_configuration_char_add			(p_uwbrange, p_uwbrange_init);
	err_code |= role_configuration_char_add			(p_uwbrange, p_uwbrange_init);
	err_code |= avg_configuration_char_add			(p_uwbrange, p_uwbrange_init);
	err_code |= threshold_char_add 					(p_uwbrange, p_uwbrange_init);
	err_code |= uwbrange_status_char_add			(p_uwbrange, p_uwbrange_init);
	err_code |= uwbrange_controls_char_add			(p_uwbrange, p_uwbrange_init);
	
	return err_code;
}

/**@brief Update the uwbrange_instant_measurement characteristic and send the value via notification
 *
 * @param[in]	p_uwbrange  pointer to the service structure
 * @param[in]	uwbrange   	uwbrange value in double
 */
uint32_t ble_uwbrange_instant_measurement_update(ble_uwbrange_t * p_uwbrange, double distance)
{
	uint32_t err_code = NRF_SUCCESS;
	ble_gatts_value_t gatts_value;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
	
	// Initialize value struct.
	memset(&gatts_value, 0, sizeof(gatts_value));

	gatts_value.len     = sizeof(double);
	gatts_value.offset  = 0;
	gatts_value.p_value = (uint8_t*)&distance;

	 // Update database.
	err_code = sd_ble_gatts_value_set(p_uwbrange->conn_handle,
                                      p_uwbrange->uwbrange_inst_measurement_handles.value_handle,
                                      &gatts_value);
		
	VERIFY_SUCCESS(err_code);
		
	// Send value if connected and notifying
	if ((p_uwbrange->conn_handle != BLE_CONN_HANDLE_INVALID) && p_uwbrange->is_notification_supported)
	{
		ble_gatts_hvx_params_t hvx_params;
            
		memset(&hvx_params, 0, sizeof(hvx_params));

		hvx_params.handle   = p_uwbrange->uwbrange_inst_measurement_handles.value_handle;
		hvx_params.type     = BLE_GATT_HVX_NOTIFICATION;
		hvx_params.offset   = 0;
		hvx_params.p_len    = &gatts_value.len;
		hvx_params.p_data   = (uint8_t*) &distance;
            
		err_code = sd_ble_gatts_hvx(p_uwbrange->conn_handle, &hvx_params);
	}
	else
	{
		err_code = NRF_ERROR_INVALID_STATE;
	}
   
    return err_code;
}

/**@brief Update the uwbrange_avg_measurement characteristic and send the value via notification
 *
 * @param[in]	p_uwbrange  pointer to the service structure
 * @param[in]	distance   	distance value in double
 */
uint32_t ble_uwbrange_avg_measurement_update(ble_uwbrange_t * p_uwbrange, double distance)
{
	uint32_t err_code = NRF_SUCCESS;
	ble_gatts_value_t gatts_value;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
	
	// Initialize value struct.
	memset(&gatts_value, 0, sizeof(gatts_value));

	gatts_value.len     = sizeof(double);
	gatts_value.offset  = 0;
	gatts_value.p_value = (uint8_t*)&distance;

	 // Update database.
	err_code = sd_ble_gatts_value_set(p_uwbrange->conn_handle,
                                      p_uwbrange->uwbrange_avg_measurement_handles.value_handle,
                                      &gatts_value);	

	VERIFY_SUCCESS(err_code);
		
	// Send value if connected and notifying
	if ((p_uwbrange->conn_handle != BLE_CONN_HANDLE_INVALID) && p_uwbrange->is_notification_supported)
	{
		ble_gatts_hvx_params_t hvx_params;
            
		memset(&hvx_params, 0, sizeof(hvx_params));

		hvx_params.handle   = p_uwbrange->uwbrange_avg_measurement_handles.value_handle;
		hvx_params.type     = BLE_GATT_HVX_NOTIFICATION;
		hvx_params.offset   = 0;
		hvx_params.p_len    = &gatts_value.len;
		hvx_params.p_data   = (uint8_t*) &distance;
            
		err_code = sd_ble_gatts_hvx(p_uwbrange->conn_handle, &hvx_params);
	}
	else
	{
		err_code = NRF_ERROR_INVALID_STATE;
	}
   
    return err_code;
}

/**@brief Update the uwbrange_event characteristic and send the value via notification
 *
 * @param[in]	p_uwbrange  pointer to the service structure
 * @param[in]	events   	events value 
 */
uint32_t ble_uwbrange_status_update(ble_uwbrange_t * p_uwbrange, uint8_t events)
{
	uint32_t err_code = NRF_SUCCESS;
	ble_gatts_value_t gatts_value;
	
	VERIFY_PARAM_NOT_NULL(p_uwbrange);
	
	// Initialize value struct.
	memset(&gatts_value, 0, sizeof(gatts_value));

	gatts_value.len     = sizeof(uint8_t);
	gatts_value.offset  = 0;
	gatts_value.p_value = (uint8_t*)&events;

	 // Update database.
	err_code = sd_ble_gatts_value_set(p_uwbrange->conn_handle,
                                      p_uwbrange->status_handles.value_handle,
                                      &gatts_value);
		
	VERIFY_SUCCESS(err_code);
		
	// Send value if connected and notifying
	if ((p_uwbrange->conn_handle != BLE_CONN_HANDLE_INVALID) && p_uwbrange->is_notification_supported)
	{
		ble_gatts_hvx_params_t hvx_params;     
		memset(&hvx_params, 0, sizeof(hvx_params));
		hvx_params.handle   = p_uwbrange->status_handles.value_handle;
		hvx_params.type     = BLE_GATT_HVX_NOTIFICATION;
		hvx_params.offset   = 0;
		hvx_params.p_len    = &gatts_value.len;
		hvx_params.p_data   = (uint8_t*) &events;
            
		err_code = sd_ble_gatts_hvx(p_uwbrange->conn_handle, &hvx_params);
	}
	else
	{
		err_code = NRF_ERROR_INVALID_STATE;
	}
   
    return err_code;
}
