// Copyright (c) 2023 Ziga Miklosic
// All Rights Reserved
////////////////////////////////////////////////////////////////////////////////
/**
*@file      ble_c.c
*@brief     Bluetooth Centrol drivers
*@author    Ziga Miklosic
*@date      10.01.2023
*@version   V1.0.0
*
*@note      This file shall be in following directory:
*           
*               /drivers/peripheral/ble
*/
////////////////////////////////////////////////////////////////////////////////
/*!
* @addtogroup BLE_C
* @{ <!-- BEGIN GROUP -->
*/
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include "project_config.h"

#include "ble_c.h"

// For softdevice
#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_soc.h"

// BLE queue manager to stack
#include "nrf_ble_qwr.h"

// GATT
#include "nrf_ble_gatt.h"

// For scanning & advertismeent
#include "ble_advdata.h"
#include "nrf_ble_scan.h"

// FIFO buffer
#include "middleware/ring_buffer/src/ring_buffer.h"

// Debug COM port
#include "middleware/cli/cli/src/cli.h" 

// nRF5 SDK Configurations
#include "sdk_config.h"
#include "app_util_platform.h"

// TODO: Remove only for debugging
#include "middleware/parameters/parameters/src/par.h"


////////////////////////////////////////////////////////////////////////////////
// Definitions
////////////////////////////////////////////////////////////////////////////////

/**
 *      BLE Central connection tag
 *
 * @brief   Connection config tag is a unique key for keeping track of an 
 *          advertising configuration
 *
 *          Further details: https://devzone.nordicsemi.com/f/nordic-q-a/33504/what-does-app_ble_conn_cfg_tag-do 
 */ 
#define BLE_C_CONN_CFG_TAG                          ( 1 )

/**
 *      Main BLE Central event priority
 *
 * @note    This is observer priority not the CPU interrupt priority!!!
 */
#define BLE_C_OBSERVER_PRIORITY                        0

/**
 *		BLE Central transmissinon/reception buffer size
 *
 * @note    Each connection have pair of Tx & Rx buffer, therefore
 *          consume more memory.
 *
 *	Unit: byte
 */                     
#define BLE_C_RX_BUF_SIZE                           ( 256 )  
#define BLE_C_TX_BUF_SIZE                           ( 256 ) 

/**
 *		BLE Central observer buffer size
 *
 * @note    Only one observer buffer exsist and it is not 
 *          related to number of active connections!
 *
 *	Unit: Multiple of <sizeof( ble_c_obs_data_t ) = 42 byte>
 */                     
#define BLE_C_OBSERVER_BUF_SIZE                     ( 32 )

/**
 *      PHY selection
 *  
 * @brief   Select PHY based on "BLE_GAP_PHYS" defines: 
 *              1. BLE_GAP_PHY_1MBPS
 *              2. BLE_GAP_PHY_CODED (LR)
 *
 * @note    For long range select BLE_GAP_PHY_CODED!
 * @note    BLE_GAP_PHY_2MBPS is not supported by this module!
 */
#define BLE_C_PHY                                  ( PROJECT_CONFIG_BLE_PHY )

 /**
  *     Scanning inteval
  * 
  * @brief  Scan interval dictates time between two consequtive
  *         scaning events.
  *
  *         Valid value range: 5 ms - 40 sec
  *         Resolution: 0.625 ms
  *
  *     Unit: ms
  */
#define BLE_C_SCAN_INTERVAL_MS                       ( 100.0 )

 /**
  *     Scanning window
  * 
  * @brief  Scan interval dictates time between two consequtive
  *         scaning events.
  *
  *         Valid value range: 5 ms - 40 sec
  *         Resolution: 0.625 ms
  *
  * @note   If scan_phys contains both @ref BLE_GAP_PHY_1MBPS and
  *         @ref BLE_GAP_PHY_CODED interval shall be larger than or
  *         equal to twice the scan window.
  *
  *     Unit: ms
  */
#define BLE_C_SCAN_WINDOW_MS                        ( 100.0 )

/**
 *      Scanning duration
 *
 * @brief   Scan duration dictates time till scanning
 *          is being terminated.
 *
 *         Valid value range: 10 ms - 655 sec
 *         Resolution: 10 ms
 *
 * @note    Value = 0 --> Continous scanning!  
 *   
 *  Unit: ms
 */
#define BLE_C_SCAN_DURATION_MS                      ( 0 )

/**
 *      Minimum connection interval
 *
 * @brief   Valid value range: 7.5 ms - 4.0 sec
 *          Resolution:  1.25 ms
 *
 * @note    Min. connection interval shall not be larger than max.!
 *
 *  Unit: ms
 */
#define BLE_C_MIN_CONN_INTERVAL_MS                  ( 100 ) 

/**
 *      Maximum connection interval
 *
 * @brief   Valid value range: 7.5 ms - 4.0 sec
 *          Resolution:  1.25 ms
 *
 * @note    Min. connection interval shall not be larger than max.!
 *
 *  Unit: ms
 */
#define BLE_C_MAX_CONN_INTERVAL_MS                  ( 200 ) 

/**
 *      Slave latency
 *
 * @brief   The slave latency value defines the number of connection
 *          events it can safely skip.
 *
 *          Valid value range: 0 - ((connSupervisionTimeout / connIntervalMax) - 1)
 *
 * @note    Value of 0 means that Peripheral BLE device needs to address
 *          every connection event triggered by Central BLE device.
 */
#define BLE_C_SLAVE_LATENCY                         ( 0 )

/**
 *      Supervsision timeout
 *
 * @brief   Valid value range: 100 ms - 32 sec
 *
 * @note    Additional condition to check:
 *
 *              SupervisionTimeout > (( 1 + SlaveLatency ) * connInterval * 2 )
 *
 *  Unit: ms
 */
#define BLE_C_SUPERVISION_TIMEOUT_MS                ( 2000 )

/**
 *      Predefined characteristics & descriptior handles
 *
 *  @note   This values are taken from fixed BLE peripheral 
 *          device via nRF Connect PC tool. If BLE peripheral 
 *          device change characteristics this values will be invalid
 *          and might cause malformed data exhange!
 *
 *  @note   Proper way is to use characteristics discovery option!
 */
#define BLE_C_TX_CHAR_HANDLE                        ( 0x10 )
#define BLE_C_TX_CHAR_CCCD_HANDLE                   ( 0x11 )
#define BLE_C_RX_CHAR_HANDLE                        ( 0x13 )
#define BLE_C_FW_VER_CHAR_HANDLE                    ( 0x16 )
#define BLE_C_HW_VER_CHAR_HANDLE                    ( 0x18 )
#define BLE_C_SER_NUM_CHAR_HANDLE                   ( 0x1A )
#define BLE_C_MAN_NAME_CHAR_HANDLE                  ( 0x1C )

/**
 *      Size of Tx and Rx characteristics
 *
 *      Valid range: 0 - NRF_SDH_BLE_GATT_MAX_MTU_SIZE
 *
 *  @note   "NRF_SDH_BLE_GATT_MAX_MTU_SIZE" macro is defined inside 
 *          sdk_config.h (nRF_SoftDevice->NRG_SDH_BLE_ENABLED->BLE Stack configuration)
 *
 *  Unit: byte
 */
#define BLE_C_CHAR_TX_RX_MTU_SIZE                   ( 240 )

/**
 *      Advertising manufacturer data aka. Magic Request value
 *
 * @note    With that value Central BLE device will filter out
 *          devices.
 */
#define BLE_C_ADV_MAGIC_REQUEST_VAL                 ( "\xAA\x55\xFF\x00\x69\x96\xCA\xFE" )
#define BLE_C_ADV_MAGIC_REQUEST_SIZE                ( 8UL ) 

/**
 *      Maximum number of peripheral connections
 */
#define BLE_C_MAX_CONNECTIONS                       ( 9 )

/**
 *      Enable/Disable start of scanning on disconnection event
 */
#define BLE_C_START_SCAN_ON_DISCONNECT               ( 1 )

/**
 *      Enable/Disable stop scanning on maximum connection reached event
 *
 *  0 - Scanning will not be stopped on max. connected device
 *  1 - Scanning will automatically stopped on final connected device (see "BLE_C_MAX_CONNECTIONS") 
 *  
 */
#define BLE_C_STOP_SCAN_ON_MAX_CONN_REACHED          ( 1 )

/**
 * 	Enable/Disable debug mode
 *
 * 	@note	Disable in release!
 */
#define BLE_C_DEBUG_EN                              ( 0 )

#ifndef DEBUG
    #undef BLE_C_DEBUG_EN
    #define BLE_C_DEBUG_EN 0
#endif

/**
 *  Enable/Disable assertion
 *
 *  @note   Disble in release!
 */
 #define BLE_C_ASSERT_EN                            ( 1 )

#ifndef DEBUG
    #undef BLE_C_ASSERT_EN
    #define BLE_C_ASSERT_EN 0
#endif

/**
 * 	Debug communication port macros
 */
#if ( 1 == BLE_C_DEBUG_EN )
	#define BLE_C_DBG_PRINT( ... )                  ( cli_printf_ch( eCLI_CH_BLE_C, __VA_ARGS__ ))
#else
	#define BLE_C_DBG_PRINT( ... )                  { ; }
#endif

/**
 *  Assertion definition
 */
 #if ( BLE_C_ASSERT_EN )
	#define BLE_C_ASSERT(x)                         { PROJECT_CONFIG_ASSERT(x) }
 #else
  #define BLE_C_ASSERT                               { ; }
 #endif


/**
 *  BLE Central connection 
 */
typedef struct
{
    p_ring_buffer_t     rx_buf;                 /**<Reception buffer */
    p_ring_buffer_t     tx_buf;                 /**<Transmission buffer */
    uint8_t             mac[6];                 /**<MAC address of connected peer */
    uint16_t            conn_handle;            /**<Connection Handle on which event occurred */
    ble_c_conn_state_t  state;                  /**<Connection state */
    uint8_t             role;                   /**<GAP role of connection */
    uint16_t            interval;               /**<Connection interval */
    uint16_t            slave_latency;          /**<Slave latency */
    uint16_t            supervisor_timeout;     /**<Supervisor timeout */
    bool                tx_in_progress;         /**<BLE transmitting in progress flag */
    bool                rssi_in_progress;       /**<RSSI measurement in progress */ 
} ble_c_conn_t;

/**
 *      BLE Central data
 */
typedef struct
{
    p_ring_buffer_t         observer_buf;                                           /**<Observer buffer */
    ble_gap_scan_params_t   scan_params;                                            /**<Scanning parameters */         
    ble_data_t              scan_data;                                              /**<Scan data (advertisment info )*/
    uint8_t                 scan_data_buffer[BLE_GAP_SCAN_BUFFER_EXTENDED_MIN];     /**<Scan data buffer where advertising reports will be stored by the SoftDevice */      
    ble_gap_conn_params_t   conn_params;                                            /**<Connection parameters */
    bool                    is_scan;                                                /**<Scanning active flag */ 
    bool                    observer_en;                                            /**<Observer enable switch */
    ble_c_conn_t            conn[BLE_C_MAX_CONNECTIONS];                            /**<Connection info of all links */
    uint8_t                 conn_num;                                               /**<Number of active connection links */
} ble_c_data_t;

/**
 *      BLE event pointer function
 */
typedef void (*pf_ble_evt)(ble_evt_t const * p_ble_evt);

/**
 *      BLE event handler registration data
 */
typedef struct
{
    pf_ble_evt      pf_handle;  /**<BLE event handler function */
    uint16_t        evt_type;   /**<BLE event type */
} ble_c_evt_hndl_t;

 #if ( 1 == BLE_C_DEBUG_EN )
    /**
     *      PHY strings
     */
    typedef struct
    {
        const char *    phy_str;
        uint8_t         phy;
    } ble_c_phy_str_t;
#endif

/**
 *  Invalid configuration catcher
 */

 // Scanning interval must be in valid range
_Static_assert(( BLE_C_SCAN_INTERVAL_MS >= 5 ) && ( BLE_C_SCAN_INTERVAL_MS <= 40000 ));

 // Scanning window must be in valid range
_Static_assert(( BLE_C_SCAN_WINDOW_MS >= 5 ) && ( BLE_C_SCAN_WINDOW_MS <= 40000 ));

// Advertisement duration must be in valid range
_Static_assert((( BLE_C_SCAN_DURATION_MS >= 10 ) && ( BLE_C_SCAN_DURATION_MS <= 655000 )) || ( 0 == BLE_C_SCAN_DURATION_MS ));

// MTU size
// NOTE: Max. value is 250 bytes, as is defined in sdk_config.h (nRF_SoftDevice->NRG_SDH_BLE_ENABLED->BLE Stack configuration)
// For some unknown reason "NRF_SDH_BLE_GATT_MAX_MTU_SIZE" does not reflect sdk_config.h
_Static_assert(( BLE_C_CHAR_TX_RX_MTU_SIZE >= 0) && ( BLE_C_CHAR_TX_RX_MTU_SIZE < 250 ));

// Min. connection interval must be smaller than max.
_Static_assert( BLE_C_MIN_CONN_INTERVAL_MS < BLE_C_MAX_CONN_INTERVAL_MS );

// Slave lateny must be in valid range
_Static_assert(( BLE_C_SLAVE_LATENCY >= 0 ) && ( BLE_C_SLAVE_LATENCY < 32 ));

// Supervision timeout must be in valid range
_Static_assert(( BLE_C_SUPERVISION_TIMEOUT_MS >= 100 ) && ( BLE_C_SUPERVISION_TIMEOUT_MS <= 32000 ));

// Supervision timeout additional condition must be meet
_Static_assert( BLE_C_SUPERVISION_TIMEOUT_MS > (( 1 + BLE_C_SLAVE_LATENCY ) * 2 * BLE_C_MAX_CONN_INTERVAL_MS ));

// Maximum number of connection must be according to nRF5 SDK configurations
_Static_assert(( BLE_C_MAX_CONNECTIONS <= NRF_SDH_BLE_CENTRAL_LINK_COUNT ) && ( BLE_C_MAX_CONNECTIONS > 0 ));


////////////////////////////////////////////////////////////////////////////////
// Variables
////////////////////////////////////////////////////////////////////////////////

/**
 *      Initialization guards
 */
static bool gb_is_init = false;

/**
 *      BLE Central data
 */
static ble_c_data_t g_ble_c = 
{ 
    .is_scan            = false,
    .observer_en        = false,
    
    // Setup scanning parameters
    .scan_params =
    {
        .active          = 0,                                                // NOTE: Passive scannig (without scan response)
        .interval        = (uint16_t) ( BLE_C_SCAN_INTERVAL_MS / 0.625f ),
        .window          = (uint16_t) ( BLE_C_SCAN_WINDOW_MS / 0.625f ),
        .timeout         = (uint16_t) ( BLE_C_SCAN_DURATION_MS / 10.0f ),
        .filter_policy   = BLE_GAP_SCAN_FP_ACCEPT_ALL,

        // Setup PHY
        #if ( BLE_GAP_PHY_1MBPS == BLE_C_PHY )
            .scan_phys     = BLE_GAP_PHY_1MBPS,
            .extended      = 0x00,
        #elif ( BLE_GAP_PHY_CODED == BLE_C_PHY )
            .scan_phys   = BLE_GAP_PHY_CODED,
            .extended    = 1,
        #else
            #error "BLE_C: Invalid selection of PHY! Check \"BLE_C_PHY\" macro !"
        #endif
    },

    // Connection parameters
    .conn_params = 
    {
        .min_conn_interval   = (uint16_t) MSEC_TO_UNITS( BLE_C_MIN_CONN_INTERVAL_MS, UNIT_1_25_MS ),
        .max_conn_interval   = (uint16_t) MSEC_TO_UNITS( BLE_C_MAX_CONN_INTERVAL_MS, UNIT_1_25_MS ),
        .slave_latency       = BLE_C_SLAVE_LATENCY,
        .conn_sup_timeout    = (uint16_t) MSEC_TO_UNITS( BLE_C_SUPERVISION_TIMEOUT_MS, UNIT_10_MS ),
    },

    // Setup scan data buffer
    .scan_data_buffer = {0},
    .scan_data = 
    {
        .p_data = (uint8_t*) &g_ble_c.scan_data_buffer,
        .len    = BLE_GAP_SCAN_BUFFER_EXTENDED_MIN,
    },

    // Connection link informations
    .conn       = {0},
    .conn_num   = 0U,

    // Observer buffer
    .observer_buf = NULL,
};  

/**
 *      Define GATT instance
 *
 *  @note   Calling this macro also register observer in background!
 */
NRF_BLE_GATT_DEF( g_gatt_instance );

/**
 *      BLE Observer Context
 *
 * @note    Context to BLE observer is being used to detect if BLE init function is being
 *          called. As observer handler is being registered by pre-processor (all done my 
 *          macros), therefore we cannot disable/cancel registration in run-time. 
 *
 *          So if device GAP role is selected in run-time (via external coding resistance)
 *          to be a peripheral device, central init function will not be called and "observer_en"
 *          switch will not be turned ON. Therefore BLE observer handler will have no impact!
 *
 *          This is workaround must be done because:
 *				1. BLE functionalities are being splitted into two translation units (ble_c, ble_p)
 *				2. nRF5 SDK registration of BLE observers is being done in compile-time
 */
static bool * const p_context = &g_ble_c.observer_en;

#if ( 1 == BLE_C_DEBUG_EN )

    /**
     *      BLE Device role
     */
    static const char * gs_ble_role[] = 
    {
        "INVALID",
        "PERIPHERAL",
        "CENTRAL",
    };

    /**
     *      BLE Device role
     */
    static const char * gs_ble_phy[] = 
    {
        "PHY_AUTO",
        "PHY_1MBPS",
        "PHY_2MBPS",
        "PHY_CODED",
        "PHY_NOT_SET",
    };

    /**
     *      PHY strings
     */
    static const ble_c_phy_str_t g_phy_str[] = 
    {
        { .phy = BLE_GAP_PHY_AUTO,      .phy_str = "PHY_AUTO"       },
        { .phy = BLE_GAP_PHY_1MBPS,     .phy_str = "PHY_1MBPS"      },
        { .phy = BLE_GAP_PHY_2MBPS,     .phy_str = "PHY_2MBPS"      },
        { .phy = BLE_GAP_PHY_CODED,     .phy_str = "PHY_CODED"      },
        { .phy = BLE_GAP_PHY_NOT_SET,   .phy_str = "PHY_NOT_SET"    },
    };
    
    /**
     *  Number of all PHY descriptions
     */
    static const uint8_t gu8_phy_str_num_of = (uint8_t)( sizeof(g_phy_str) / sizeof(ble_c_phy_str_t));

#endif


////////////////////////////////////////////////////////////////////////////////
// Function prototypes
////////////////////////////////////////////////////////////////////////////////
static void             ble_c_conn_data_init                (const ble_c_conn_par_t * const p_conn_params);
static void             ble_c_scan_data_init                (const ble_c_scan_par_t * const p_scan_params);
static ble_c_status_t   ble_c_buffers_init                  (void);
static ble_c_status_t   ble_c_stack_init                    (void);
static void             ble_c_evt_hndl                      (ble_evt_t const * p_ble_evt, void * p_context);
static inline void      ble_c_evt_on_connect                (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_disconnect             (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_update_phy             (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_conn_param_update_req  (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_missing_attr           (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_gap_timeout            (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_gattc_timeout          (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_gatts_timeout          (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_adv_report             (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_rx_cmpt                (ble_evt_t const * p_ble_evt);
static inline void      ble_c_evt_on_tx_cmpt                (ble_evt_t const * p_ble_evt);
static ble_c_status_t   ble_c_gatt_init                     (void);
static ble_c_status_t   ble_c_get_manufacturer_data         (const uint8_t * const p_adv_data, const uint16_t adv_len, uint8_t * const p_man_data, uint16_t * const p_man_data_len);
static ble_c_status_t	ble_c_set_tx_cccd_value			    (const uint16_t conn_num, const bool cccd);
static ble_c_status_t   ble_c_send                          (const uint16_t conn_handle, const uint8_t * const p_data, const uint16_t len);
static bool             ble_c_check_magic_request           (const uint8_t * const p_man_data, const uint16_t size);
static bool             ble_c_check_dev_conn                (const uint8_t * const p_mac);
static bool             ble_c_conn_data_get_free            (uint8_t * const p_idx);
static bool             ble_c_conn_data_get_idx             (const uint16_t conn_handle, uint8_t * const p_idx);
static ble_c_status_t   ble_c_check_conn_params             (const ble_c_conn_par_t * const p_conn_params);

#if ( 1 == BLE_C_DEBUG_EN )
    static const char*      ble_c_get_phy_str               (const uint8_t phy);
#endif


/**
 *      List of all registered BLE events
 */
static const ble_c_evt_hndl_t g_ble_c_hndl[] = 
{
    { .evt_type = BLE_GAP_EVT_CONNECTED,                    .pf_handle = ble_c_evt_on_connect               },
    { .evt_type = BLE_GAP_EVT_DISCONNECTED,                 .pf_handle = ble_c_evt_on_disconnect            },
    { .evt_type = BLE_GAP_EVT_PHY_UPDATE_REQUEST,           .pf_handle = ble_c_evt_on_update_phy            },
    { .evt_type = BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST,    .pf_handle = ble_c_evt_on_conn_param_update_req },
    { .evt_type = BLE_GATTS_EVT_SYS_ATTR_MISSING,           .pf_handle = ble_c_evt_on_missing_attr          },
    { .evt_type = BLE_GAP_EVT_TIMEOUT,                      .pf_handle = ble_c_evt_on_gap_timeout           },
    { .evt_type = BLE_GATTC_EVT_TIMEOUT,                    .pf_handle = ble_c_evt_on_gattc_timeout         },
    { .evt_type = BLE_GATTS_EVT_TIMEOUT,                    .pf_handle = ble_c_evt_on_gatts_timeout         },
    { .evt_type = BLE_GAP_EVT_ADV_REPORT,                   .pf_handle = ble_c_evt_on_adv_report            },
    { .evt_type = BLE_GATTC_EVT_HVX,                        .pf_handle = ble_c_evt_on_rx_cmpt               },
    { .evt_type = BLE_GATTC_EVT_WRITE_CMD_TX_COMPLETE,      .pf_handle = ble_c_evt_on_tx_cmpt               },
};

/**
 *      Number of all registered events
 */
static const uint32_t gu32_ble_hndl_num_of = (uint32_t) ( sizeof( g_ble_c_hndl ) / sizeof( ble_c_evt_hndl_t ));


////////////////////////////////////////////////////////////////////////////////
// Functions
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/**
*		Initialize BLE Central connection parameters
*
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static void ble_c_conn_data_init(const ble_c_conn_par_t * const p_conn_params)
{
    // Initialize connection data
    for ( uint8_t conn_idx = 0; conn_idx < BLE_C_MAX_CONNECTIONS; conn_idx++ )
    {
        g_ble_c.conn[ conn_idx ].conn_handle = BLE_CONN_HANDLE_INVALID;
        g_ble_c.conn[ conn_idx ].state       = eBLE_C_CONN_UNKNOWN;
    }  
    
    // Requested non-default connection parameters
    if ( NULL != p_conn_params )
    {
        // Check if connection parameter are valid
        if ( eBLE_C_OK == ble_c_check_conn_params( p_conn_params ))
        {
            // Change default connection paramters
            g_ble_c.conn_params.min_conn_interval  = MSEC_TO_UNITS( p_conn_params->conn_inteval, UNIT_1_25_MS);
            g_ble_c.conn_params.max_conn_interval  = MSEC_TO_UNITS( p_conn_params->conn_inteval, UNIT_1_25_MS);
            g_ble_c.conn_params.slave_latency      = p_conn_params->slave_latency;
            g_ble_c.conn_params.conn_sup_timeout   = MSEC_TO_UNITS( p_conn_params->supervision_timeout, UNIT_10_MS);

            BLE_C_DBG_PRINT( "Change default connection parameters!" );
        }
    }   
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Initialize BLE Central scannign parameters
*
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static void ble_c_scan_data_init(const ble_c_scan_par_t * const p_scan_params)
{
    // Requested non-default scanning parameters
    if ( NULL != p_scan_params )
    {
        g_ble_c.scan_params.interval    = (uint16_t) ( p_scan_params->interval / 0.625f );
        g_ble_c.scan_params.window      = (uint16_t) ( p_scan_params->window / 0.625f );
        g_ble_c.scan_params.timeout     = (uint16_t) ( p_scan_params->duration / 10.0f );
    }  
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Initialize BLE Central Tx/Rx buffers
*
* @return 		status - Status of initialization
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_buffers_init(void)
{
    ble_c_status_t status = eBLE_C_OK;
    
    // Observer FIFO settings
    const ring_buffer_attr_t buf_obs_attr = 
    { 	
        .item_size 	= sizeof( ble_c_obs_data_t ),   // Observer data size
        .override 	= true,                         // Lose old data
        .p_mem      = NULL,                         // Allocate dynamically!
    };

    // Init observer buffer
    if ( eRING_BUFFER_OK != ring_buffer_init( &g_ble_c.observer_buf, BLE_C_OBSERVER_BUF_SIZE, &buf_obs_attr ))
    {
        status = eBLE_C_ERROR;
    }

    // Init buffer dynamically, do not allow lose of data (override=false)
    const ring_buffer_attr_t buf_attr = 
    { 	
        .item_size 	= sizeof(uint8_t),
        .override 	= false,
    };
    
    // Initialize all expected connection buffers
    for ( uint8_t conn_idx = 0; conn_idx < BLE_C_MAX_CONNECTIONS; conn_idx++ )
    {
        // Init transmission buffer
        if ( eRING_BUFFER_OK != ring_buffer_init( &g_ble_c.conn[conn_idx].tx_buf, BLE_C_TX_BUF_SIZE, &buf_attr ))
        {
            status = eBLE_C_ERROR;
            break;
        } 

        // Init reception buffer
        if ( eRING_BUFFER_OK != ring_buffer_init( &g_ble_c.conn[conn_idx].rx_buf, BLE_C_RX_BUF_SIZE, &buf_attr ))
        {
            status = eBLE_C_ERROR;
            break;
        } 
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Initialize BLE peripheral stack
*
* @return 		status - Status of initialization
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_stack_init(void)
{
    ble_c_status_t  status      = eBLE_C_OK;
    uint32_t        ram_start   = 0;  // Application RAM start address

    // Start SoftDevice
    if ( NRF_SUCCESS != nrf_sdh_enable_request())
    {
        status = eBLE_C_ERROR;
        
        BLE_C_DBG_PRINT( "SoftDevice enable request error!" );
        BLE_C_ASSERT( 0 ); 
    }

    // Configure BLE stack
    if ( NRF_SUCCESS != nrf_sdh_ble_default_cfg_set( BLE_C_CONN_CFG_TAG, &ram_start ))
    {
        status = eBLE_C_ERROR;
        
        BLE_C_DBG_PRINT( "SoftDevice default config set error!" );
        BLE_C_ASSERT( 0 ); 
    }

    // Enable BLE stack
    const ret_code_t status_sd = nrf_sdh_ble_enable( &ram_start );

    #if ( 1 == BLE_C_DEBUG_EN )

        // Not enoght memory for softdevice
        if ( NRF_ERROR_NO_MEM == status_sd )
        {        
            BLE_C_DBG_PRINT("Insufficient RAM allocated for the SoftDevice.");
        
            // Calculate total RAM size
            const uint32_t ram_total_size = (uint32_t)( 0x20000000UL + ( NRF_FICR->INFO.RAM * 1024UL ));
        
            // Calculate maximum size for application RAM
            const uint32_t app_ram_size = (uint32_t)( ram_total_size - ram_start );

            BLE_C_DBG_PRINT("Change the RAM start location to 0x%08X", ram_start );
            BLE_C_DBG_PRINT("Maximum RAM size for application is 0x%08X (%d kB)", app_ram_size, ( app_ram_size / 1024UL ));      
        }
    #endif

    if ( NRF_SUCCESS != status_sd )
    {
        status = eBLE_C_ERROR;
        
        BLE_C_DBG_PRINT( "SoftDevice enable error!" );
        BLE_C_ASSERT( 0 ); 
    }

    // Enable observer
    g_ble_c.observer_en = true;
    
    // Register BLE event callback
    // An observer is essentially a piece of code that listens for events.
    NRF_SDH_BLE_OBSERVER( m_ble_c_observer, BLE_C_OBSERVER_PRIORITY, ble_c_evt_hndl, (void*) p_context );

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Main BLE event handler
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @param[in] 	p_context   - Pointer to context
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static void ble_c_evt_hndl(ble_evt_t const * p_ble_evt, void * p_context)
{
    // Get context
    const bool * const p_observer_enable = p_context;

    if ( NULL != p_observer_enable )
    {
        // Observer turned ON
        if ( true == *p_observer_enable )
        {
            // Go thru registered events
            for ( uint32_t handler = 0; handler < gu32_ble_hndl_num_of; handler++ )
            {
                // Is event registered
                if ( g_ble_c_hndl[handler].evt_type == p_ble_evt->header.evt_id )
                {
                    // Execute assigned handler
                    g_ble_c_hndl[handler].pf_handle( p_ble_evt );

                    break;
                }
            }
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on connect event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_connect(ble_evt_t const * p_ble_evt)
{
    ble_c_evt_t event       = { 0 };
    uint8_t     conn_idx    = 0U;
    
    /**
     *      UNDERSTANDING CONNECTION HANDLE ASSIGNMENT IN MULTILINK CASES    
     *  
     *      Connection handle is assigned at every connection event by SoftDevice.  It is 
     *      continuous as long as no devices disconnects. I.e. the first device will get 
     *      handle 0, device number 2 gets handle 1, and so on. But if the first device 
     *      disconnects, then device number 3 might get handle 0 again.
     *
     *
     * @note    Furthermore, due to the supervision timeout mechanism, it might in some cases 
     *          take some time before the Softdevice detects that a device has lost its connection. 
     *          And if the same device then tries to reconnect quickly (before the supervision timeout occurs) 
     *          it might get assigned a new connection handle. Hence, it is actually possible that 
     *          the same device has two connection handles at the same time, although only until 
     *          the supervision timeout occurs.   
     *  
     *          Source: https://devzone.nordicsemi.com/f/nordic-q-a/28143/ble-connection-handle-value-scope
     */

    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;

    // Get connection info
    const ble_gap_evt_connected_t * const p_connected = &( p_ble_evt->evt.gap_evt.params.connected );

    // Is there empty slot in connection data
    if ( true == ble_c_conn_data_get_free( &conn_idx ))
    {
        // New connection count
        g_ble_c.conn_num++;

        // Setup connection data
        g_ble_c.conn[ conn_idx ].conn_handle        = conn_handle;
        g_ble_c.conn[ conn_idx ].state              = eBLE_C_CONN_CONNECTED;
        g_ble_c.conn[ conn_idx ].role               = p_connected->role;
        g_ble_c.conn[ conn_idx ].interval           = (uint16_t)( p_connected->conn_params.max_conn_interval * 1.25f );
        g_ble_c.conn[ conn_idx ].slave_latency      = (uint16_t)( p_connected->conn_params.slave_latency );
        g_ble_c.conn[ conn_idx ].supervisor_timeout = (uint16_t)( p_connected->conn_params.conn_sup_timeout * 10UL );
        
        // Copy MAC address
        memcpy( &g_ble_c.conn[ conn_idx ].mac, p_connected->peer_addr.addr, sizeof( g_ble_c.conn[ conn_idx ].mac ));
      
        // Clear buffers
        (void) ble_c_clear_buffers( conn_idx );

        // Enable CCCD on Peripheral device for Tx characteristics
        (void) ble_c_set_tx_cccd_value( conn_idx, true );

        // Prepare event data
        event.type           = eBLE_C_EVT_CONNECT;
        event.conn_handle    = conn_handle;
        event.conn_num       = conn_idx;
        memcpy( &event.peer_mac, p_connected->peer_addr.addr, sizeof( event.peer_mac ));

        // Raise callback
        ble_c_evt_cb( event );

        // Start scanning if there is still room to make a new connection
        if ( g_ble_c.conn_num < BLE_C_MAX_CONNECTIONS )
        {
            (void) ble_c_scan_start();
        }

        // Maximum number of connection reached -> no need for futher scanning
        #if ( 1 == BLE_C_STOP_SCAN_ON_MAX_CONN_REACHED ) 
            else
            {
                (void) ble_c_scan_stop();

                BLE_C_DBG_PRINT( "Scanning stopped due to maximum number of links!" );
            }
        #endif

        // Debug informations...
        BLE_C_DBG_PRINT( "------------------------------------------------------------" );
        BLE_C_DBG_PRINT( "New connection established! Link count: %d", g_ble_c.conn_num );
        BLE_C_DBG_PRINT( "   Connection handle: %d", conn_handle );
        BLE_C_DBG_PRINT( "                Role: %s", gs_ble_role[p_ble_evt->evt.gap_evt.params.connected.role]  );
        BLE_C_DBG_PRINT( "            Peer MAC: %02X:%02X:%02X:%02X:%02X:%02X", g_ble_c.conn[ conn_idx ].mac[5], g_ble_c.conn[ conn_idx ].mac[4], g_ble_c.conn[ conn_idx ].mac[3],
                                                                                g_ble_c.conn[ conn_idx ].mac[2], g_ble_c.conn[ conn_idx ].mac[1], g_ble_c.conn[ conn_idx ].mac[0] );
        BLE_C_DBG_PRINT( " Connection interval: %d ms", g_ble_c.conn[ conn_idx ].interval);
        BLE_C_DBG_PRINT( "       Slave latency: %d", g_ble_c.conn[ conn_idx ].slave_latency );
        BLE_C_DBG_PRINT( "  Supervisor Timeout: %d ms", g_ble_c.conn[ conn_idx ].supervisor_timeout);
        BLE_C_DBG_PRINT( "------------------------------------------------------------" );
    }
#if ( 1 == BLE_C_DEBUG_EN )
    else
    {
        BLE_C_DBG_PRINT( "No free space in connection data buffer!" );
        BLE_C_ASSERT( 0 );
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on disconnect event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_disconnect(ble_evt_t const * p_ble_evt)
{
    ble_c_evt_t event       = { 0 };
    uint8_t     conn_idx    = 0U;

    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;

    // Find connection data by connection handle
    if  ( true == ble_c_conn_data_get_idx( conn_handle, &conn_idx ))
    {
        // Connection lost count 
        g_ble_c.conn_num--;
        
        // Setup state and connection handle
        g_ble_c.conn[ conn_idx ].state          = eBLE_C_CONN_DISCONNECTED;
        g_ble_c.conn[ conn_idx ].conn_handle    = BLE_CONN_HANDLE_INVALID;

        // Clear in progress flags
        g_ble_c.conn[ conn_idx ].rssi_in_progress = false;
        g_ble_c.conn[ conn_idx ].tx_in_progress = false;

        // Prepare event data
        event.type           = eBLE_C_EVT_DISCONNECT;
        event.conn_handle    = conn_handle;
        event.conn_num       = conn_idx;
        memcpy( &event.peer_mac, &g_ble_c.conn[ conn_idx ].mac, sizeof( event.peer_mac ));

        // Raise callback
        ble_c_evt_cb( event );

        // Clear MAC
        memset( &g_ble_c.conn[ conn_idx ].mac, 0U, sizeof( g_ble_c.conn[ conn_idx ].mac ));

        // Debug informations...
        BLE_C_DBG_PRINT( "------------------------------------------------------------" );
        BLE_C_DBG_PRINT( "Connection lost! Link count: %d", g_ble_c.conn_num );
        BLE_C_DBG_PRINT( "  Connection handle: %d", conn_handle );
        BLE_C_DBG_PRINT( "------------------------------------------------------------" );

        // Start scanning after disconnection
        #if ( 1 == BLE_C_START_SCAN_ON_DISCONNECT )
            
           // sd_ble_gap_scan_stop();
            
            (void) ble_c_scan_start();
        #endif
    }
#if ( 1 == BLE_C_DEBUG_EN )
    else
    {
        BLE_C_DBG_PRINT( "Connection handle (%d) assigned by Soft Device not expected or connection count invalid!", conn_handle );
        BLE_C_ASSERT( 0 );
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on PHY Update Procedure complete event handler 
*
* @note     This function is being executed in main BLE stack callback!
*      
* @note    This event must be handler in order not to lose connection 
*          with peer. If this event is not handled the connection
*          will timeout.
*
*          Further details: https://infocenter.nordicsemi.com/index.jsp?topic=%2Fcom.nordic.infocenter.s132.api.v7.2.0%2Fgroup___b_l_e___g_a_p___p_e_r_i_p_h_e_r_a_l___p_h_y___u_p_d_a_t_e.html&cp=4_7_3_1_2_1_5_7_1
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_update_phy(ble_evt_t const * p_ble_evt)
{
    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;

    // Set to auto option for PHY
    const ble_gap_phys_t phys = 
    {
        .rx_phys = BLE_GAP_PHY_AUTO,
        .tx_phys = BLE_GAP_PHY_AUTO,
    };

    // Update PHY
    if ( NRF_SUCCESS != sd_ble_gap_phy_update( conn_handle, &phys))
    {
        // TODO: How to handle this error???

        BLE_C_DBG_PRINT( "PHY update error!" );
        BLE_C_ASSERT( 0 );
    }
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on Connection Parameter update request from peripheral
*
* @note     This function is being executed in main BLE stack callback!
*      
* @note     This event is triggered by receiving connection parameter update
*           from peripheral device. Then it is up to central device to decide
*           how to proceed with that request. It can eihter ignore, accept
*           or reject wanted connection parameters.
*
*          Further details: https://devzone.nordicsemi.com/f/nordic-q-a/8736/how-to-handle-ble_gap_evt_conn_param_update-and-ble_gap_evt_conn_param_update_request-events
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_conn_param_update_req(ble_evt_t const * p_ble_evt)
{
    // No actions...
    // Ignore connection parameter update from peripheral device...
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on missing system attributes event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @note    This event is triggered if the peer request a read on any of the
*          system attributes
*
*          In the case where you send indications or notifications before the peer requests 
*          such a read, for instance you want to perform a "sd_ble_gatts_service_changed()", 
*          you need to do "the sd_ble_gatts_sys_attr_set()" first.
*
*          Furhter details: https://devzone.nordicsemi.com/f/nordic-q-a/54039/why-don-t-i-get-a-ble_gatts_evt_sys_attr_missing-event
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_missing_attr(ble_evt_t const * p_ble_evt)
{
    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;

    // Update persistent system attribute information
    if ( NRF_SUCCESS != sd_ble_gatts_sys_attr_set( conn_handle, NULL, 0, 0))
    {
        // TODO: How to handle this error???

        BLE_C_DBG_PRINT( "BLE_P: Setting SYSTEM_ATTRIBUTES error!" );
        BLE_C_ASSERT( 0 );
    }    
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on GAP timeout event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_gap_timeout(ble_evt_t const * p_ble_evt)
{
    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gap_evt.conn_handle;

    // Based on timeout event
    switch( p_ble_evt->evt.gap_evt.params.timeout.src )
    {
        /**< Scanning timeouted */
        case BLE_GAP_TIMEOUT_SRC_SCAN:

            // Scanning stopped
            g_ble_c.is_scan = false; 

            break;

        /**< Connection timeoued. */
        case BLE_GAP_TIMEOUT_SRC_CONN:

        /**< Authenticated payload timeout. */
        case BLE_GAP_TIMEOUT_SRC_AUTH_PAYLOAD:
        default:
            // No actions...
            break;
    }
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on GATT Client timeout event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_gattc_timeout(ble_evt_t const * p_ble_evt)
{
    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;

    // Disconnect on GATT Client timeout event.
    BLE_C_DBG_PRINT("GATT Client Timeout.");

    // Disconnect
    if ( NRF_SUCCESS != sd_ble_gap_disconnect( conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION ))
    {
        BLE_C_DBG_PRINT( "GAP disconnect error! " );
        BLE_C_ASSERT( 0 );  
    }
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on GATT Server timeout event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_gatts_timeout(ble_evt_t const * p_ble_evt)
{
    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;

    // Disconnect on GATT Server timeout event.
    BLE_C_DBG_PRINT("GATT Server Timeout.");

    // Disconnect
    if ( NRF_SUCCESS != sd_ble_gap_disconnect( conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION ))
    {   
        BLE_C_DBG_PRINT( "GAP disconnect error! " );
        BLE_C_ASSERT( 0 );
    }
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on advertisement report event handler 
*   
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_adv_report(ble_evt_t const * p_ble_evt)
{
    static  uint8_t             man_data[32]    = { 0 };
            uint16_t            man_data_len    = 0;
             ble_c_obs_data_t   observer_data   = { 0 };

    // Get advertisment info
    const ble_gap_evt_adv_report_t * const p_adv_report = &( p_ble_evt->evt.gap_evt.params.adv_report );

    // Get advertisement type
    const ble_gap_adv_report_type_t * const p_adv_type = &( p_adv_report->type );

    // Get advertisement data and lenght
    const uint8_t * p_adv_data      = p_adv_report->data.p_data;
    const uint16_t  adv_data_len    = p_adv_report->data.len;

    // Advertisment status OK
    if ( BLE_GAP_ADV_DATA_STATUS_COMPLETE == p_adv_type->status )
    {
        // Get manufacturer data
        if ( eBLE_C_OK == ble_c_get_manufacturer_data( p_adv_data, adv_data_len, (uint8_t*) &man_data, &man_data_len ))
        {
            // Advertisment request for connection
            if ( 1U == p_adv_type->connectable )
            {
                BLE_C_DBG_PRINT( "Connectable advertising report!" );

                // Is this device already connected
                if ( false == ble_c_check_dev_conn( p_adv_report->peer_addr.addr ))
                {                      
                    // Check for magic request
                    if ( true == ble_c_check_magic_request( man_data, man_data_len ))
                    {
                        // Stop scanning before connection
                        (void) sd_ble_gap_scan_stop();

                        // Connect to peer device
                        if ( NRF_SUCCESS != sd_ble_gap_connect( &p_adv_report->peer_addr, &g_ble_c.scan_params, &g_ble_c.conn_params, BLE_C_CONN_CFG_TAG ))
                        {
                            BLE_C_DBG_PRINT( "Attept to connect failed!" );
                        }
                    }
                }
            }

            // Advertisement only broadcast
            else
            {
                BLE_C_DBG_PRINT( "Broadcasting advertising report!" );

                // Assemble observer data packet
                observer_data.rssi = p_adv_report->rssi;
                observer_data.size = man_data_len;
                memcpy( &observer_data.data, &man_data, man_data_len );
                memcpy( &observer_data.mac, p_adv_report->peer_addr.addr, sizeof( observer_data.mac ));
            
                // Put data to observer data
                if ( eRING_BUFFER_OK != ring_buffer_add( g_ble_c.observer_buf, (ble_c_obs_data_t*) &observer_data ))
                {
                    // Buffer overflow!
                    BLE_C_DBG_PRINT( "Observer buffer overflow! Increse buffer size via \"BLE_C_OBSERVER_BUF_SIZE\" macro!");
                }
            }
        }
    }

    // Continue scanning
    (void) sd_ble_gap_scan_start( NULL, &g_ble_c.scan_data );

    // Check if this is needed
    // Raise callback
    //ble_c_evt_cb( eBLE_C_EVT_ADV_RX );  
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on data reception event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_rx_cmpt(ble_evt_t const * p_ble_evt)
{
    ble_c_evt_t event       = { 0 };
    uint8_t     conn_idx    = 0U;

    // Get GATTC event info
    const ble_gattc_evt_t * const p_gattc_evt = &( p_ble_evt->evt.gattc_evt );

    // Get connection handle
    const uint16_t conn_handle = p_gattc_evt->conn_handle;

    // Find connection data by connection handle
    if  ( true == ble_c_conn_data_get_idx( conn_handle, &conn_idx ))
    {
        // GATT status code for the operation
        if ( BLE_GATT_STATUS_SUCCESS == p_gattc_evt->gatt_status )
        {
            // Get HVX info
            const ble_gattc_evt_hvx_t * const p_hvx = &( p_ble_evt->evt.gattc_evt.params.hvx );
            
            // Is msg from Server Tx characteristics
            if ( BLE_C_TX_CHAR_HANDLE == p_hvx->handle )
            {
                // Accept only notifications
                if ( BLE_GATT_HVX_NOTIFICATION == p_hvx->type )
                {
                    // Get data and lenght
                    const uint8_t * p_data  = p_hvx->data;
                    const uint16_t len      = p_hvx->len;

                    // Add to RX fifo
                    for ( uint16_t idx = 0; idx < len; idx++ )
                    {
                        if ( eRING_BUFFER_OK != ring_buffer_add( g_ble_c.conn[ conn_idx ].rx_buf, (uint8_t*) &p_data[idx] ))
                        {
                            // Buffer overflow!
                            BLE_C_DBG_PRINT( "Rx buffer overflow! Increse buffer size via \"BLE_C_RX_BUF_SIZE\" macro!");

                            break;
                        }
                    }

                    // Prepare event data
                    event.conn_handle   = conn_handle;
                    event.type          = eBLE_C_EVT_RX_DATA;
                    memcpy( &event.peer_mac, g_ble_c.conn[ conn_idx ].mac, sizeof( event.peer_mac ));

                    // Raise callback
                    ble_c_evt_cb( event );

                    // Debug informations...
                    BLE_C_DBG_PRINT( "Msg received on connection handle %d with size of %d bytes!", conn_handle, len );
                }
            }
        }
    }
#if ( 1 == BLE_C_DEBUG_EN )
    else
    {
        BLE_C_DBG_PRINT( "Connection handle not found!" );
        BLE_C_ASSERT( 0 );
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE on write complete event handler 
*
* @note     This function is being executed in main BLE stack callback!
*
* @param[in] 	p_ble_evt   - Pointer to BLE event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
static inline void ble_c_evt_on_tx_cmpt(ble_evt_t const * p_ble_evt)
{
            uint32_t    tx_buf_taken                    = 0UL;
    static  uint8_t     data[BLE_C_CHAR_TX_RX_MTU_SIZE] = {0};
            uint8_t     conn_idx                        = 0U;

    // Get connection handle
    const uint16_t conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;

    // Find connection data by connection handle
    if  ( true == ble_c_conn_data_get_idx( conn_handle, &conn_idx ))
    {
        // Get number of taken places inside buffer
        (void) ring_buffer_get_taken( g_ble_c.conn[ conn_idx ].tx_buf, &tx_buf_taken );

        // Tx Buffer empty -> nothing to send 
        if ( 0UL == tx_buf_taken )
        {
            // Clear Tx in progress flag
            g_ble_c.conn[ conn_idx ].tx_in_progress = false;
        }

        // Tx buffer not empty -> send what is inside
        else
        {
            uint8_t msg_len = 0U;

            // First byte in Tx buffer is msg size
            (void) ring_buffer_get( g_ble_c.conn[ conn_idx ].tx_buf, (uint8_t*) &msg_len );

            // Take all message bytes
            for ( uint8_t msg_idx = 0; msg_idx < (const uint8_t) msg_len; msg_idx++ )
            {
                // Take byte from Tx buffer
                if ( eRING_BUFFER_EMPTY == ring_buffer_get( g_ble_c.conn[ conn_idx ].tx_buf, (uint8_t*) &data[msg_idx]))
                {
                    // Shall not ended up here, as we put "msg_len" number of bytes in TX buffer in "ble_transmit" function!
                    // Major fuck up!
                    BLE_C_ASSERT( 0 );
                }  
            }
    
            // Push data to client
            (void) ble_c_send( conn_handle, (const uint8_t*) &data, msg_len );
        }
    }
#if ( 1 == BLE_C_DEBUG_EN )
    else
    {
        BLE_C_DBG_PRINT( "Connection handle not found!" );
        BLE_C_ASSERT( 0 );
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Initialize BLE Central GATT
*
* @return 		status - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_gatt_init(void)
{
    ble_c_status_t status = eBLE_C_OK;

    // Initialize GATT
    if ( NRF_SUCCESS != nrf_ble_gatt_init( &g_gatt_instance, NULL ))
    { 
        status = eBLE_C_ERROR;

        BLE_C_DBG_PRINT( "GATT init error!" );  
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Get manufacturer data from advertisement packet
*
* @param[in]    p_adv_data      - Pointer to advertisement data
* @param[in]    adv_len         - Lenght of advertisement data
* @param[out]   p_man_data      - Pointer to parsed manufacturer data    
* @param[out]   p_man_data_len  - Pointer to manufacturer data lenght  
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_get_manufacturer_data(const uint8_t * const p_adv_data, const uint16_t adv_len, uint8_t * const p_man_data, uint16_t * const p_man_data_len)
{
    ble_c_status_t  status  = eBLE_C_OK;
    uint16_t        offset  = 0;

    // Search for manufacturer data in advertising packet
    *p_man_data_len = ble_advdata_search( p_adv_data, adv_len, &offset, BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA );

    // Man data found
    if ( *p_man_data_len > 0 )
    {   
        // Subtract two chars from lenght
        // NOTE: -2 due to Company ID in manufacturer specific data
        (*p_man_data_len) = ((*p_man_data_len) - 2U );

        // Copy manufacturer data
        // NOTE: +2 as ignoring Company ID in manufacturer specific data
        memcpy( p_man_data, &p_adv_data[ ( offset + 2U ) ], *p_man_data_len );
    }

    // Manufacturer data missing
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Enable/Disable CCCD for TX characteristics on peer GATT Server
*
* @param[in] 	conn_num		- Connection number
* @param[in] 	cccd            - Value of CCCD
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_set_tx_cccd_value(const uint16_t conn_num, const bool cccd)
{
    ble_c_status_t  status  = eBLE_C_OK;
    bool            is_conn = true;

    // Get connection flag
    (void) ble_c_is_connected( conn_num, &is_conn );

    BLE_C_ASSERT( true == is_conn );
    
    // Check if connection is established
    if ( true == is_conn )
    {
        // Enable CCCD for Tx characteristics on Peripheral device
        ble_gattc_write_params_t write_params = {0};

        write_params.write_op   = BLE_GATT_OP_WRITE_CMD;                    /**< Write Operation to be performed, see @ref BLE_GATT_WRITE_OPS. */
        write_params.flags      = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_WRITE;  /**< Flags, see @ref BLE_GATT_EXEC_WRITE_FLAGS. */
        write_params.handle     = BLE_C_TX_CHAR_CCCD_HANDLE;                /**< Handle to the attribute to be written. */
        write_params.offset     = 0;                                        /**< Offset in bytes. @note For WRITE_CMD and WRITE_REQ, offset must be 0. */
        write_params.len        = 1;                                        /**< Length of data in bytes. */
        write_params.p_value    = (uint8_t*) &cccd;                          /**< Pointer to the value data. */

        // Write CCCD
        if ( NRF_SUCCESS != sd_ble_gattc_write( g_ble_c.conn[ conn_num ].conn_handle, &write_params ))
        {
            status = eBLE_C_ERROR;

            BLE_C_DBG_PRINT( "Writing to TX CCCD error!" );
        }  
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Send data to Client GATT server	
*
* @note     This function writes to RX characteristics on Client GATT Server!
*
* @param[in]    conn_handle     - Connection handle
* @param[in]    p_data          - Pointer to data to notify
* @param[in]    len             - Lenght of notification data
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_send(const uint16_t conn_handle, const uint8_t * const p_data, const uint16_t len)
{
    ble_c_status_t              status          = eBLE_C_OK;
    ble_gattc_write_params_t    write_params    = {0};

    // Set up write parameters
    write_params.write_op   = BLE_GATT_OP_WRITE_CMD;                    /**< Write Operation to be performed, see @ref BLE_GATT_WRITE_OPS. */
    write_params.flags      = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_WRITE;  /**< Flags, see @ref BLE_GATT_EXEC_WRITE_FLAGS. */
    write_params.handle     = BLE_C_RX_CHAR_HANDLE;                     /**< Handle to the attribute to be written. */
    write_params.offset     = 0;                                        /**< Offset in bytes. @note For WRITE_CMD and WRITE_REQ, offset must be 0. */
    write_params.len        = len;                                       
    write_params.p_value    = (uint8_t*) p_data;                          

    // Push data to server RC characteristics
    if ( NRF_SUCCESS != sd_ble_gattc_write( conn_handle, &write_params ))
    {
        status = eBLE_C_ERROR;

        BLE_C_DBG_PRINT( "Writing to RX characteristics error on conn_handle: %d!", conn_handle );
    }  

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Check for magic request from peripheral device
*
* @param[in]    p_man_data  - Pointer to manufacturer data inside advertisement packet
* @param[in]    size        - Size of manufacturer data
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
static bool ble_c_check_magic_request(const uint8_t * const p_man_data, const uint16_t size)
{
    bool            is_magic_request   = true;
    const uint8_t * p_magic_request    = (const uint8_t*) BLE_C_ADV_MAGIC_REQUEST_VAL;

    // First check for correct size
    if ( BLE_C_ADV_MAGIC_REQUEST_SIZE == size )
    {
        // Check for data
        for ( uint32_t ch = 0; ch < BLE_C_ADV_MAGIC_REQUEST_SIZE; ch++ )
        {
            // Check byte per byte
            if ( p_magic_request[ch] != p_man_data[ch] )
            {
                // Magic request not valid
                is_magic_request = false;

                break;
            }
        }
    }

    // Size invalid
    else
    {
        is_magic_request = false;
    }

    return is_magic_request;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Check if device is already connected
*
* @note     This check is being used for disabling double-connections with
*           the same peer device!
*
* @param[in]    p_mac           - Device MAC address
* @return 		is_dev_conn     - True if device is already connected
*/
////////////////////////////////////////////////////////////////////////////////
static bool ble_c_check_dev_conn(const uint8_t * const p_mac)
{
    bool is_dev_conn = false;

    for ( uint8_t conn_idx = 0; conn_idx < BLE_C_MAX_CONNECTIONS; conn_idx++ )
    {
        // Check only connected devices
        if ( eBLE_C_CONN_CONNECTED == g_ble_c.conn[ conn_idx ].state )
        {
            // Check for maching MAC
            if ( 0U == memcmp( &g_ble_c.conn[ conn_idx ].mac, p_mac, sizeof( g_ble_c.conn[ conn_idx ].mac )))
            {
                is_dev_conn = true;
                break;
            }
        }
    }

    return is_dev_conn;
}


////////////////////////////////////////////////////////////////////////////////
/**
*       Check for free slot in connection data buffer
*
* @note     All connection data are stored in global variable: "g_ble_c.conn[]"
*
* @param[out]    p_idx       - Pointer to index with empty slot
* @return 		is_free     - True if available space for new connection data
*/
////////////////////////////////////////////////////////////////////////////////
static bool ble_c_conn_data_get_free(uint8_t * const p_idx)
{
    bool is_free = false;

    // Go thru connection data
    for ( uint8_t idx = 0; idx < BLE_C_MAX_CONNECTIONS; idx++)
    {
        // Find first available free slot
        if ( eBLE_C_CONN_CONNECTED != g_ble_c.conn[ idx ].state )
        {
            *p_idx = idx;
            is_free = true;

            break;
        }
    }

    return is_free;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Get connection data buffer index based on connection handle value
*
* @note     All connection data are stored in global variable: "g_ble_c.conn[]"
*
* @param[in]    conn_handle - Target connection handle
* @param[out]   p_idx       - Pointer to index with empty slot
* @return 		is_free     - True if available space for new connection data
*/
////////////////////////////////////////////////////////////////////////////////
static bool ble_c_conn_data_get_idx(const uint16_t conn_handle, uint8_t * const p_idx)
{
    bool idx_found = false;

    // Go thru connection data
    for ( uint8_t idx = 0; idx < BLE_C_MAX_CONNECTIONS; idx++)
    {
        // Founded target connection data
        if ( conn_handle == g_ble_c.conn[ idx ].conn_handle )
        {
            *p_idx = idx;
            idx_found = true;

            break;
        }
    }

    return idx_found;
}

////////////////////////////////////////////////////////////////////////////////
/**
*       Check if connection parameters are in valid range
*
* @param[in]    p_conn_params   - Pointer to connection parameters
* @return 		status          - OK if connection parameters inside valid range
*/
////////////////////////////////////////////////////////////////////////////////
static ble_c_status_t ble_c_check_conn_params(const ble_c_conn_par_t * const p_conn_params)
{
    ble_c_status_t status = eBLE_C_ERROR;

    // Check for valid connection paramters
    if  (   ( p_conn_params->conn_inteval >= 7.5f )
        &&  ( p_conn_params->conn_inteval <= 4000.0f )
        &&  ( p_conn_params->slave_latency <= 32U )
        &&  ( p_conn_params->supervision_timeout >= 100U )
        &&  ( p_conn_params->supervision_timeout <= 32000U )
        &&  ( p_conn_params->supervision_timeout > ((( 1 + p_conn_params->slave_latency ) * 2 * p_conn_params->conn_inteval ))))
    {
        status = eBLE_C_OK;
    }
    else
    {
         BLE_C_DBG_PRINT( "Invalid connection parameters!" );  
    }

    return status;
} 

#if ( 1 == BLE_C_DEBUG_EN )

	////////////////////////////////////////////////////////////////////////////////
	/**
	*		Get attribute type string description
	*
	* @param[in]	cmd_type	- SCP attribute type
	* @return		str			- SCP attribute type description
	*/
	////////////////////////////////////////////////////////////////////////////////
    static const char* ble_c_get_phy_str(const uint8_t phy)
	{
		uint16_t i = 0;
		const char * str = "N/A";

		for ( i = 0; i < gu8_phy_str_num_of; i++ )
		{
			if ( phy == g_phy_str[i].phy )
			{
				str =  (const char*) g_phy_str[i].phy_str;
				break;
			}
		}

		return str;
	}

#endif

////////////////////////////////////////////////////////////////////////////////
/**
* @} <!-- END GROUP -->
*/
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/**
*@addtogroup BLE_C_API
* @{ <!-- BEGIN GROUP -->
*
* 	Following function are part of BLE Central API.
*/
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central Initialize
*
* @brief    BLE central module expects custom service and with two 
*           characteristics on Peripheral side:
*
*               1. TX Characteristics: for Peripheral TX, Central RX (Central In Peripheral Out - CIPO)
*               2. RX Characteristics: for Peripheral RX, Central TX (Central Out Peripheral In - COPI)
*
*           Basic client-server model:
*
*                           write to RX characteristic
*               CLIENT -----------------------------------> SERVER
*        (central BLE device)                        (peripheral BLE device)
*
*
*                        notification to TX characteristic
*               CLIENT <----------------------------------- SERVER
*        (central BLE device)                        (peripheral BLE device)
*
*
*           Implemented communication sheme is emulating simple UART interface.
*
*       
* @note     TX Characteristics is defined as notification, therefore client
*           must first enable CCCD (Client Characteristics Configuration Descriptor)
*           before it can receive notifications!
*
* @param[in]    p_conn_params   - Pointer to connection parameters
* @param[in]    p_scan_params   - Pointer to scanning parameters
* @param[in]    scan_start      - Start scanning
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_init(const ble_c_conn_par_t * const p_conn_params, const ble_c_scan_par_t * const p_scan_params, const bool scan_start)
{
    ble_c_status_t status = eBLE_C_OK;

    if ( false == gb_is_init )
    {
        // Init connection & scanning parameters
        ble_c_conn_data_init( p_conn_params );
        ble_c_scan_data_init( p_scan_params );

        // Init Tx/Rx buffers
        status |= ble_c_buffers_init();

        // Init BLE stack
        status |= ble_c_stack_init();

        // Init GATT
        status |= ble_c_gatt_init();     

        // Init success
        if ( eBLE_C_OK == status )
        {
            gb_is_init = true;

            // Start scanning    
            if ( true == scan_start )
            {
                status = ble_c_scan_start();
            }
        }
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central get init status
*
* @param[out] 	p_is_conn   - Connection status, true if initiated
* @return 		status		- Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_is_init(bool * const p_is_init)
{
    ble_c_status_t status = eBLE_C_OK;

    if ( NULL != p_is_init )
    {
        *p_is_init = gb_is_init;
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;  
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central start scanning
*
* @brief    This function setup scanning timings and must be called before
*           calling scanning continue function.
*
* @return       status - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_scan_start(void)
{
    ble_c_status_t  status  = eBLE_C_OK;
    ble_c_evt_t     event   = {0};

    BLE_C_ASSERT( true == gb_is_init );

    if ( true == gb_is_init )
    {
        // Enter critical
        CRITICAL_REGION_ENTER();

        // Stop scanning 
        // NOTE: According to multilink example!
        (void) sd_ble_gap_scan_stop();

        // Start scannig
        if ( NRF_SUCCESS == sd_ble_gap_scan_start( &g_ble_c.scan_params, &g_ble_c.scan_data ))
        {
            // Scan started OK -> set scan status flag
            g_ble_c.is_scan = true;
        }

        // Exit critical
        CRITICAL_REGION_EXIT();

        // Raise callback
        event.type = eBLE_C_EVT_SCAN_START;
        ble_c_evt_cb( event );

        // Debug print
        #if ( 1 == BLE_C_DEBUG_EN )
            if ( true == g_ble_c.is_scan )
            {
                BLE_C_DBG_PRINT( "Scanning started!" );
            }
            else
            {
                BLE_C_DBG_PRINT( "Scanning starting error!" );
            }
        #endif

    }
    else
    {
        status = eBLE_C_ERROR;  
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central stop scanning
*
* @return       status - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_scan_stop(void)
{
    ble_c_status_t  status  = eBLE_C_OK;
    ble_c_evt_t     event   = { 0 };

    BLE_C_ASSERT( true == gb_is_init );

    if ( true == gb_is_init )
    {
        // Enter critical
        CRITICAL_REGION_ENTER();

        /**
         *      Stop scanning
         *
         * @note    Ignore return state as it can return SUCCESS or ERROR_INVALID_STATE both
         *          meaning that scanning is no longer in progress!
         */
        (void) sd_ble_gap_scan_stop();

        // Scanning stopped -> clear scan status flag
        g_ble_c.is_scan = false;

        // Exit critical
        CRITICAL_REGION_EXIT();

        // Raise callback
        event.type = eBLE_C_EVT_SCAN_END;
        ble_c_evt_cb( event );
    }
    else
    {
        status = eBLE_C_ERROR;  
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Is scanning in progress 
*
* @param[out]   p_is_scan   - Pointer to scanning in progress flag
* @return       status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_is_scan(bool * const p_is_scan)
{
    ble_c_status_t status = eBLE_C_OK;
    
    if ( NULL != p_is_scan )
    {
        *p_is_scan = g_ble_c.is_scan;
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Update connection parameters
*
* @note     Default connection parameters remains the same as passed at init 
*           function!
*
* @predonditions     
*
*   1.  The connection must already be established between the devices.
*
*   2.  The central device (i.e., the device initiating the connection 
*       parameter update) must have an active BLE connection with the peripheral device.
*
*   3.  The peripheral device (i.e., the device receiving the connection parameter update request) 
*       must support the Connection Parameters Update Procedure, as specified in the Bluetooth Core Specification.
*
*   4.  The requested connection parameters must be within the limits supported by both devices. 
*       The BLE stack on both devices must support the requested connection parameters, including 
*       the connection interval, slave latency, and connection timeout.
*
*   5.  The connection parameter update procedure can only be initiated by the central device. 
*       The peripheral device cannot initiate a connection parameter update.
*
* @param[in]    conn_num        - Connection number
* @param[in]    p_conn_params   - Pointer to connection parameters
* @return       status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_change_conn_param(const uint16_t conn_num, const ble_c_conn_par_t * const p_conn_params)
{
    ble_c_status_t          status      = eBLE_C_OK;
    ble_gap_conn_params_t   conn_params = {0};

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( NULL != p_conn_params );

    if  (   ( true == gb_is_init )
        &&  ( NULL != p_conn_params )
        &&  ( eBLE_C_CONN_CONNECTED == g_ble_c.conn[conn_num].state ))
    {
        // Check connection parameters
        if ( eBLE_C_OK == ble_c_check_conn_params( p_conn_params ))
        {
            // Change current connection parameters
            conn_params.min_conn_interval  = MSEC_TO_UNITS( p_conn_params->conn_inteval, UNIT_1_25_MS);
            conn_params.max_conn_interval  = MSEC_TO_UNITS( p_conn_params->conn_inteval, UNIT_1_25_MS);
            conn_params.slave_latency      = p_conn_params->slave_latency;
            conn_params.conn_sup_timeout   = MSEC_TO_UNITS( p_conn_params->supervision_timeout, UNIT_10_MS);

            // Continue scanning
            if ( NRF_SUCCESS != sd_ble_gap_conn_param_update( g_ble_c.conn[conn_num].conn_handle, &conn_params ))
            {
                status = eBLE_C_ERROR;

                BLE_C_DBG_PRINT( "Connection update error on conn_num: %d", conn_num );
            }        
        }
        else
        {
            status = eBLE_C_ERROR;
        }
    }
    else
    {
        status = eBLE_C_ERROR;
    }  

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Is Peripheral device connected
*
* @param[in]    conn_num    - Connection number
* @param[out]   p_is_conn   - Pointer to connection flag
* @return       status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_is_connected(const uint16_t conn_num, bool * const p_is_conn)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( NULL != p_is_conn );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );

    if  (   ( NULL != p_is_conn )
        &&  ( conn_num < BLE_C_MAX_CONNECTIONS ))
    {
        if ( eBLE_C_CONN_CONNECTED == g_ble_c.conn[ conn_num ].state )
        {
            *p_is_conn = true;
        }
        else
        {
            *p_is_conn = false; 
        }
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;   
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central transmit data
*	
* @note     This function push data to GATT Server to Rx characteristics.
*
* @note     This function shall not be called if connection is not established!
*
* @note     If transmission is already in progress data will be stored into
*           Tx FIFO in form of:
*
*                   [msg_len|msg_data], 
*           
*               -> msg_len:     Length of "msg_data" in bytes, size: 1 byte
*               -> msg_data:    Message data field, min size: 1 byte, max size: MTU bytes
*
*           Tx FIFO message is being encoded with length in order not to fracture
*           BLE messages over the radio. Meaning that receiver will always receive
*           complete message transmited with API call: ble_transmit
*
*
* @param[in]    conn_num    - Connection number
* @param[in] 	p_data      - Pointer to data for transmit
* @param[in] 	len         - Lenght of data in bytes
* @return 		status		- Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_transmit(const uint16_t conn_num, const uint8_t * const p_data, const uint16_t len)
{
    ble_c_status_t  status          = eBLE_C_OK;
    bool            is_connected    = false;
    
    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );
	BLE_C_ASSERT( NULL != p_data );
	BLE_C_ASSERT(( len > 0U ) && ( len <= BLE_C_CHAR_TX_RX_MTU_SIZE ));
    
	if	(	( true == gb_is_init ) 
		&&	( conn_num < BLE_C_MAX_CONNECTIONS ))
    {
        // Get connection status
        (void) ble_c_is_connected( conn_num, &is_connected );

        BLE_C_ASSERT( true == is_connected );

    	if	(   ( true == is_connected )
            &&  ( NULL != p_data )
            &&  (( len > 0U ) && ( len <= BLE_C_CHAR_TX_RX_MTU_SIZE )))
    	{
            // Tx idle 
            if ( false == g_ble_c.conn[ conn_num ].tx_in_progress )
            {
                // Send data to server
                status = ble_c_send( g_ble_c.conn[ conn_num ].conn_handle, p_data, len );

                // Push data to server success
                if ( eBLE_C_OK == status )
                {
                    g_ble_c.conn[ conn_num ].tx_in_progress = true;
                }
            }

            // Tx busy -> buffer data and send after TX done event
            else
            {          
                uint32_t buf_free_space = 0U;

                // Check if there is space in Tx buffer
                (void) ring_buffer_get_free( g_ble_c.conn[ conn_num ].tx_buf, &buf_free_space );

                // There is enough space to fit complete message to Tx Buffer
                // NOTE: +1 due for adding lenght of message before data fields
                if (( len + 1U ) <= buf_free_space )
                {
                    // Enter critical
                    CRITICAL_REGION_ENTER();

                    /**
                     *      This actions shall not be interruptable in order to put message to Tx buffer completely !!!
                     *
                     *      Wihtout entering critical region "tx complete" ISR can preempted this function in between 
                     *      Tx FIFO filling!
                     */

                    // Put message lenght before actual data
                    (void) ring_buffer_add( g_ble_c.conn[ conn_num ].tx_buf, (uint8_t*) &len );

                    // Add data to Tx buffer
                    for ( uint16_t idx = 0; idx < len; idx++ )
                    {
                        if ( eRING_BUFFER_OK != ring_buffer_add( g_ble_c.conn[ conn_num ].tx_buf, (uint8_t*) &p_data[idx] ))
                        {
                            status = eBLE_C_ERROR;
                            BLE_C_DBG_PRINT( "Tx Ring buffer full! Increase Tx buffer size!" );

                            break;
                        }
                    }  

                    // Exit critical
                    CRITICAL_REGION_EXIT();
                }

                // Not enough space in buffer
                else
                {
                    status = eBLE_C_ERROR;
                } 
            }
    	}
        else
        {
            status = eBLE_C_ERROR;
        }
    }
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central read data
*	
* @note     This function retrives data from GATT Server Tx characteristics.
*
* @note     Tx characteristics is defined as notify/read, therefore only Server
*           can write to characteristics value.
*
* @note     Returned data is taken from local FIFO buffer storage. In case reception
*           FIFO is empty it return ERROR code, otherwise OK.
*
* @param[in]    conn_num    - Connection number
* @param[out] 	p_data      - Pointer to data for transmit
* @return 		status		- Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_receive(const uint16_t conn_num, uint8_t * const p_data)
{
    ble_c_status_t  status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
	BLE_C_ASSERT( NULL != p_data );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );

	if	(	( true == gb_is_init ) 
		&&	( NULL != p_data )
		&&	( conn_num < BLE_C_MAX_CONNECTIONS ))
	{
		// Get from buffer
		if ( eRING_BUFFER_OK != ring_buffer_get( g_ble_c.conn[ conn_num ].rx_buf, p_data ))
		{
			status = eBLE_C_ERROR;
		}
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central clear Tx/Rx buffers
*
* @param[in]    conn_num    - Connection number
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_clear_buffers(const uint16_t conn_num)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );

	if	(	( true == gb_is_init ) 
		&&	( conn_num < BLE_C_MAX_CONNECTIONS ))
	{
        // Reset receiving buffer
        if ( eRING_BUFFER_OK != ring_buffer_reset( g_ble_c.conn[ conn_num ].rx_buf ))
        {
            status = eBLE_C_ERROR;
        }

        // Reset transmitting buffer
        if ( eRING_BUFFER_OK != ring_buffer_reset( g_ble_c.conn[ conn_num ].tx_buf ))
        {
            status = eBLE_C_ERROR;
        }
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central get observer data
*	
* @note     This function retrives manufacturer specific data from non-connectable 
*           advertisement packets.
*
* @note     Returned data is taken from local FIFO buffer storage. In case
*           FIFO is empty it return ERROR code, otherwise OK.
*
* @param[out] 	p_data      - Pointer to observed data
* @return 		status		- Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_obs_receive(ble_c_obs_data_t * const p_data)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
	BLE_C_ASSERT( NULL != p_data );

	if	(	( true == gb_is_init ) 
		&&	( NULL != p_data ))
	{
		// Get from buffer
		if ( eRING_BUFFER_OK != ring_buffer_get( g_ble_c.observer_buf, p_data ))
		{
			status = eBLE_C_ERROR;
		}
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central clear observer buffer
*
* @return 		status - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_obs_clear_buffer(void)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );

	if ( true == gb_is_init ) 
	{
        // Reset observer buffer
        if ( eRING_BUFFER_OK != ring_buffer_reset( g_ble_c.observer_buf ))
        {
            status = eBLE_C_ERROR;
        }
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Set radio Tx power during connection
*	
* @precondition     Specified connection handle must be in connected state!
*
* @param[in]    conn_num    - Connection number
* @param[in]    tx_power    - Transmit power 
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_set_tx_power(const uint16_t conn_num, const int8_t tx_power)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );
    BLE_C_ASSERT( eBLE_C_CONN_CONNECTED == g_ble_c.conn[conn_num].state );

    // Check if valid TX power value
    BLE_C_ASSERT(   ( -40 == tx_power )
                ||  ( -20 == tx_power ) 
                ||  ( -16 == tx_power ) 
                ||  ( -12 == tx_power ) 
                ||  ( -8 == tx_power ) 
                ||  ( -4 == tx_power ) 
                ||  ( 0 == tx_power ) 
                ||  ( 2 == tx_power ) 
                ||  ( 3 == tx_power ) 
                ||  ( 4 == tx_power ) 
                ||  ( 5 == tx_power ) 
                ||  ( 6 == tx_power ) 
                ||  ( 7 == tx_power ) 
                ||  ( 8 == tx_power ));

	if	(	( true == gb_is_init ) 
		&&	( conn_num < BLE_C_MAX_CONNECTIONS )
		&&	( eBLE_C_CONN_CONNECTED == g_ble_c.conn[conn_num].state  )
        &&  (   ( -40 == tx_power )
            ||  ( -20 == tx_power ) 
            ||  ( -16 == tx_power ) 
            ||  ( -12 == tx_power ) 
            ||  ( -8 == tx_power ) 
            ||  ( -4 == tx_power ) 
            ||  ( 0 == tx_power ) 
            ||  ( 2 == tx_power ) 
            ||  ( 3 == tx_power ) 
            ||  ( 4 == tx_power ) 
            ||  ( 5 == tx_power ) 
            ||  ( 6 == tx_power ) 
            ||  ( 7 == tx_power ) 
            ||  ( 8 == tx_power )))
	{
        // Set tx power for connection
        if ( NRF_SUCCESS != sd_ble_gap_tx_power_set( BLE_GAP_TX_POWER_ROLE_CONN, g_ble_c.conn[ conn_num ].conn_handle, tx_power ))
        {
            status = eBLE_C_ERROR;

            BLE_C_DBG_PRINT( "Setting TX power error!" );
            BLE_C_ASSERT( 0 );  
        }
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Start Received Signal Strenght Indication (RSSI) measurement
*	
* @param[in]    conn_num    - Connection number
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_rssi_start(const uint16_t conn_num)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );

	if	(	( true == gb_is_init ) 
		&&	( conn_num < BLE_C_MAX_CONNECTIONS ))
	{
        if ( false == g_ble_c.conn[ conn_num ].rssi_in_progress )
        {
            // Start measurement of RSSI
            if ( NRF_SUCCESS == sd_ble_gap_rssi_start( g_ble_c.conn[ conn_num ].conn_handle, 0, 0 ))
            {   
                g_ble_c.conn[ conn_num ].rssi_in_progress = true;
            }
            else
            {
                status = eBLE_C_ERROR;
                BLE_C_DBG_PRINT( "Starting RSSI measurement error with %d handle!", g_ble_c.conn[ conn_num ].conn_handle );  
            }
        }  
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Stop Received Signal Strenght Indication (RSSI) measurement
*	
* @param[in]    conn_num    - Connection number
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_rssi_stop(const uint16_t conn_num)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );

	if	(	( true == gb_is_init ) 
		&&	( conn_num < BLE_C_MAX_CONNECTIONS ))
	{
        if ( true == g_ble_c.conn[ conn_num ].rssi_in_progress )
        {
            // Start measurement of RSSI
            if ( NRF_SUCCESS == sd_ble_gap_rssi_stop( g_ble_c.conn[ conn_num ].conn_handle ))
            {   
                g_ble_c.conn[ conn_num ].rssi_in_progress = false;
            }
            else
            {
                status = eBLE_C_ERROR;
                BLE_C_DBG_PRINT( "Stopping RSSI measurement error with %d handle!", g_ble_c.conn[ conn_num ].conn_handle );  
            }
        }     
	}
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;    
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Get Received Signal Strenght Indication (RSSI)
*	
* @param[in]    conn_num    - Connection number
* @param[out]   p_rssi      - Value of RSSI 
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_rssi_get(const uint16_t conn_num, int8_t * const p_rssi)
{
    ble_c_status_t  status          = eBLE_C_OK;
    bool            is_connected    = false;
    uint8_t         ch_index        = 0;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );
	BLE_C_ASSERT( NULL != p_rssi );

    if	(	( true == gb_is_init ) 
        &&  ( conn_num < BLE_C_MAX_CONNECTIONS )
        &&  ( NULL != p_rssi ))
	{
        // Get connection status
        (void) ble_c_is_connected( conn_num, &is_connected );

        BLE_C_ASSERT( true == is_connected );

        // If not connected does not make sense to get RSSI
    	if ( true == is_connected )
    	{
            // Start RSSI
            (void) ble_c_rssi_start( conn_num );

            // Is RSSI in progress
            if ( true == g_ble_c.conn[ conn_num ].rssi_in_progress )
            {
                // Get RSSI
                if ( NRF_SUCCESS != sd_ble_gap_rssi_get( g_ble_c.conn[ conn_num ].conn_handle, p_rssi, &ch_index ))
                {
                    status = eBLE_C_ERROR;
                }
            }
    	}
        else
        {
            status = eBLE_C_ERROR;
        }
    }
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;    
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Get active connection number for BLE Central device
*	
* @param[out]   p_active_conn   - Pointer to active connection value
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_get_active_conn(uint8_t * const p_active_conn)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( NULL != p_active_conn );

    if ( NULL != p_active_conn  ) 
	{
        *p_active_conn = g_ble_c.conn_num;
    }
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Get maximum expected connection for BLE Central device
*	
* @param[out]   p_max_conn  - Pointer to max connection value
* @return 		status      - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_get_max_conn(uint8_t * const p_max_conn)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( NULL != p_max_conn );

    if ( NULL != p_max_conn  ) 
	{
        *p_max_conn = BLE_C_MAX_CONNECTIONS;
    }
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;      
}   

////////////////////////////////////////////////////////////////////////////////
/**
*		Get current state of connection informations
*	
* @param[in]    conn_num        - Connection number
* @param[out]   p_conn_state    - Pointer to connection state info
* @param[out]   p_conn_handle   - Pointer to connection handle
* @param[out]   p_mac           - Pointer to device MAC
* @return 		status          - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_get_conn_info(const uint16_t conn_num, ble_c_conn_state_t * const p_conn_state, uint16_t * const p_conn_handle, uint8_t * const p_mac)
{
    ble_c_status_t status = eBLE_C_OK;

    BLE_C_ASSERT( true == gb_is_init );
    BLE_C_ASSERT( conn_num < BLE_C_MAX_CONNECTIONS );
    BLE_C_ASSERT( NULL != p_conn_state );
    BLE_C_ASSERT( NULL != p_conn_handle );
    BLE_C_ASSERT( NULL != p_mac );

    if	(	( true == gb_is_init ) 
        &&  ( conn_num < BLE_C_MAX_CONNECTIONS ))
	{
        // Return connection info
        *p_conn_state  = g_ble_c.conn[ conn_num ].state;
        *p_conn_handle = g_ble_c.conn[ conn_num ].conn_handle;
        memcpy( p_mac, &g_ble_c.conn[ conn_num ].mac, sizeof( g_ble_c.conn[ conn_num ].mac ));
    }
	else
	{
		status = eBLE_C_ERROR;
	}

    return status;      
}

////////////////////////////////////////////////////////////////////////////////
/**
*		Get BLE MAC Address
*
* @note     GAP address type shall be set to BLE_GAP_ADDR_TYPE_RANDOM_STATIC
*           in order to have a fixed constant MAC address, otherwise 
*           MAC can change!
*
* @param[out]   p_mac   - Pointer to MAC data	
* @return 		status  - Status of operation
*/
////////////////////////////////////////////////////////////////////////////////
ble_c_status_t ble_c_get_mac(uint8_t * const p_mac)
{
    ble_c_status_t  status  = eBLE_C_OK;
    ble_gap_addr_t  mac     = {0};

    if ( NULL != p_mac )
    {
        // Get MAC
        if ( NRF_SUCCESS == sd_ble_gap_addr_get( &mac ))
        {
            memcpy( p_mac, &mac.addr, sizeof(mac.addr));
        }
        else
        {
            status = eBLE_C_ERROR;
        }
    }
    else
    {
        status = eBLE_C_ERROR;
    }

    return status;
}

////////////////////////////////////////////////////////////////////////////////
/**
*		BLE Central events
*
* @param[in]	event	- Event informations
* @return 		void
*/
////////////////////////////////////////////////////////////////////////////////
__attribute__((weak)) void ble_c_evt_cb(const ble_c_evt_t event)
{
    /**
     * 	Leave empty for user application purposes...
     */
}

////////////////////////////////////////////////////////////////////////////////
/**
* @} <!-- END GROUP -->
*/
////////////////////////////////////////////////////////////////////////////////
