#include "ble_service.h"
#include "app_common.h"
#include "ble_modem.h"
#include "host/ble_hs.h"
#include "services/gap/ble_svc_gap.h"
#include "wm_bt_util.h"
#include "bluetooth_sig_values.h"
#include "FreeRTOS.h"

#define USABLE_DEFAULT_MTU (20) //23 - 3 of header bytes

/* ble service internal workings attributes */
static volatile ble_service_state_e _ble_service_state = BLE_SERVICE_MODE_STOPPED;
static nus_data_rx_fn_t _ble_service_nus_data_rx_cb = NULL;
static ble_service_state_change_fn_t _ble_service_state_change_cb = NULL;
static uint32_t _pairing_passkey = 0;

/* Connection handle to the connected device : only one simultaneous connection */
static uint16_t ble_device_conn_handle = BLE_HS_CONN_HANDLE_NONE;
static uint16_t usable_mtu = USABLE_DEFAULT_MTU;
static uint8_t _device_mac_address[6] = {0x00};

/**
 * @brief Structure used to store the various data to carry out a chunked notification or indication data transfer
 * 
 */
typedef struct
{
    uint16_t length; // The length of the data being sent
    uint16_t offset; // The offset used to be ableto send the next chunk of data in the array
    uint16_t sent_chunk_size; // The size of the chunk sent
    const uint8_t *data; // The address of the data to send
    bool transfer_in_progress; // Is a transfer already in progress ?
} data_being_sent_t;

// Only one transfer of a type can occur at any given time ie
// one notification and one indication can happen at the same time but not more.
static data_being_sent_t notification_data = {.data = NULL, .length = 0, .offset = 0, .transfer_in_progress = false};
static data_being_sent_t indication_data = {.data = NULL, .length = 0, .offset = 0, .transfer_in_progress = false};

static struct ble_gap_event_listener ble_gap_event_listener;

static int battery_level_char_access_cb(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg);
static uint16_t battery_level_char_handle = 0;
// Default battery value set to 100%
static uint8_t _battery_level_value = 100;


static int gatt_nus_char_access_cb(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg);
static const ble_uuid128_t gatt_nus_service_uuid = BLE_UUID128_INIT(0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5, 0x01, 0x00, 0x40, 0x6E);

static uint16_t gatt_nus_char_rx_handle = 0;
static const ble_uuid128_t gatt_nus_char_rx_uuid = BLE_UUID128_INIT(0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5, 0x02, 0x00, 0x40, 0x6E);

static uint16_t gatt_nus_char_tx_handle = 0;
static const ble_uuid128_t gatt_nus_char_tx_uuid = BLE_UUID128_INIT(0x9E, 0xCA, 0xDC, 0x24, 0x0E, 0xE5, 0xA9, 0xE0, 0x93, 0xF3, 0xA3, 0xB5, 0x03, 0x00, 0x40, 0x6E);

static struct ble_gatt_svc_def gatt_svc[] = 
{
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = BLE_UUID16_DECLARE(BLE_DEVICE_ADV_SERVICE),
        .characteristics = (struct ble_gatt_chr_def[])
        {
            {
                .uuid = BLE_UUID16_DECLARE(0x2A19),
                .val_handle = &battery_level_char_handle,
                .access_cb = &(battery_level_char_access_cb),
                .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY,
                /*.descriptors = (struct ble_gatt_dsc_def[])
                {
                    {
                        .uuid = BLE_UUID,
                        .access_cb = ,
                        .att_flags = BLE_ATT_F_READ;
                    },
                    {
                        .uuid = NULL
                    }
                }*/
            },
            {
                .uuid = NULL
            }
        }
    },
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = &gatt_nus_service_uuid.u,
        .characteristics = (struct ble_gatt_chr_def[])
        {
            {
                .uuid = &gatt_nus_char_rx_uuid.u,
                .val_handle = &gatt_nus_char_rx_handle,
                .access_cb = &(gatt_nus_char_access_cb),
                .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_WRITE_NO_RSP
            }
            ,
            {
                .uuid = &gatt_nus_char_tx_uuid.u,
                .val_handle = &gatt_nus_char_tx_handle,
                .access_cb = &(gatt_nus_char_access_cb),
                .flags = BLE_GATT_CHR_F_NOTIFY
            }
            ,
            {
                .uuid = NULL
            }
        }
    },
    {
        .type = BLE_GATT_SVC_TYPE_END
    }
};

static bool ble_service_define_gatt(const struct ble_gatt_svc_def *gatt_svc);
static bool ble_service_advertise(bool enable);
static int ble_gap_event_cb(struct ble_gap_event *event, void *arg);
static int ble_advertise_gap_event_cb(struct ble_gap_event *event, void *arg);
static int ble_conn_gap_event_cb(struct ble_gap_event *event, void *arg);
static void print_conn_desc(const struct ble_gap_conn_desc *desc);
// Raw because it doesn't handle payload fragmentation if mtu size is smaller than the payload size
static bool ble_service_send_raw_custom_notification(uint16_t characteristic_handle, const uint8_t *data, uint16_t length);
static bool ble_service_send_custom_notification(uint16_t characteristic_handle, data_being_sent_t * const data);
static void reset_data_being_sent(data_being_sent_t * const data);

// Needed to get the reponse after a mtu exchange request
static int ble_gatt_mtu_cb(uint16_t conn_handle, const struct ble_gatt_error *error, uint16_t mtu, void *arg);

const char *ble_service_state_2_str(ble_service_state_e state)
{
    switch(state) 
    {
        CASE_RETURN_STR(BLE_SERVICE_MODE_STOPPED)
        CASE_RETURN_STR(BLE_SERVICE_MODE_IDLE)
        CASE_RETURN_STR(BLE_SERVICE_MODE_ADVERTISING)
        CASE_RETURN_STR(BLE_SERVICE_MODE_CONNECTED)
        CASE_RETURN_STR(BLE_SERVICE_MODE_INDICATING)
        CASE_RETURN_STR(BLE_SERVICE_MODE_EXITING)
        default:
            return "Unkown ble service state";
    }
}

/**
 * PUBLIC FUNCTION DEFINITION
 */

bool ble_service_start(void)
{
    int status = BLE_HS_ENOERR;

    // 1 We first check if the BLE service is stopped
    if(_ble_service_state != BLE_SERVICE_MODE_STOPPED)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, ble service already running (%s)"NEW_LINE, __FUNCTION__, ble_service_state_2_str(_ble_service_state));
        return true;
    }
    
    // 2 We check if the BLE MODEM is turned on
    if(!is_ble_modem_on())
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble modem is not turned on"NEW_LINE, __FUNCTION__);
        return false;
    }

    // 3 We set our device name and appearance
    if((status = ble_svc_gap_device_name_set(BLE_DEVICE_NAME)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, ble_svc_gap_device_name_set "NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
    }

    if((status = ble_svc_gap_device_appearance_set(BLE_DEVICE_APPEARANCE)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, ble_svc_gap_device_appearance_set "NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
    }

    // 4 We register our gatt (service structure)
    if(!ble_service_define_gatt(gatt_svc))
    {
        TLS_BT_APPL_TRACE_ERROR("%s, failed to define gatt"NEW_LINE, __FUNCTION__);
        return false;
    }

    // 5 We register a gap event handler callback
    if((status = ble_gap_event_listener_register(&ble_gap_event_listener, &(ble_gap_event_cb), NULL)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_event_listener_register %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    // 6 We are now ready to make all registered services available to peers
    if((status = ble_gatts_start()) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gatts_start failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    // 7 Finally we can start advertising
    if(!ble_service_advertise(true))
    {
        return false;
    }

    _ble_service_state = BLE_SERVICE_MODE_ADVERTISING;

    return true;
}

bool ble_service_stop(void)
{
    int status = BLE_HS_ENOERR;

    // 1 We first check if the BLE service is not stopped
    if(_ble_service_state == BLE_SERVICE_MODE_STOPPED)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, ble service already stopped"NEW_LINE, __FUNCTION__);
        return true;
    }

    // 2 We check if the BLE MODEM is turned on
    if(!is_ble_modem_on())
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble modem is not turned on"NEW_LINE, __FUNCTION__);
        return false;
    }

    switch(_ble_service_state)
    {
        case BLE_SERVICE_MODE_CONNECTED:
        case BLE_SERVICE_MODE_INDICATING:
            status = ble_gap_terminate(ble_device_conn_handle, BLE_ERR_REM_USER_CONN_TERM);
            if(status == BLE_HS_ENOERR)
            {
                _ble_service_state = BLE_SERVICE_MODE_EXITING;
            }
            else //BLE_HS_EDISABLED || BLE_HS_ENOTCONN or any other error
            {
                TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_terminate %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                _ble_service_state = BLE_SERVICE_MODE_STOPPED;
                //Unregister gap event listener
                if((status = ble_gap_event_listener_unregister(&ble_gap_event_listener)) != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_event_listener_unregister %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                }
            }
            break;
        case BLE_SERVICE_MODE_ADVERTISING:
            if(!ble_service_advertise(false))
            {
                TLS_BT_APPL_TRACE_WARNING("%s, ble_service_advertise failed to stop"NEW_LINE, __FUNCTION__);
            }
            else
            {
                _ble_service_state = BLE_SERVICE_MODE_STOPPED;
                //Unregister gap event listener
                if((status = ble_gap_event_listener_unregister(&ble_gap_event_listener)) != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_event_listener_unregister %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                }
            }
            break;
        default: //Just to keep compiler happy since BLE_SERVICE_MODE_STOPPED is rulled out earlier
            break;
    }

    if(_ble_service_state == BLE_SERVICE_MODE_STOPPED)
    {
        // We finally clean the gatt registered services
        if((status = ble_gatts_reset()) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_gatts_reset %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            return false;
        }
    }
    
    return true;
}

bool ble_service_is_started(void)
{
    return _ble_service_state != BLE_SERVICE_MODE_STOPPED;
}

bool ble_service_is_device_connected(void)
{
    return _ble_service_state == BLE_SERVICE_MODE_CONNECTED;
}

const uint8_t *ble_service_get_device_mac_address(void)
{
    // We only need to read from efuse section once.
    if(
        _device_mac_address[0] == 0 &&
        _device_mac_address[1] == 0 &&
        _device_mac_address[2] == 0 &&
        _device_mac_address[3] == 0 &&
        _device_mac_address[4] == 0 &&
        _device_mac_address[5] == 0
    )
    {
        extern int tls_get_bt_mac_addr(u8 *mac);
        tls_get_bt_mac_addr(_device_mac_address);

        // Make sure the device address is compliant with the random address specification :
        _device_mac_address[5] |= 0xC0;
    }

    return _device_mac_address;
}

void ble_service_register_state_change_cb(ble_service_state_change_fn_t ble_service_state_change_cb)
{
    _ble_service_state_change_cb = ble_service_state_change_cb;
}

ble_service_state_e ble_service_get_state(void)
{
    return _ble_service_state;
}

void ble_service_set_pairing_passkey(uint32_t passkey)
{
    _pairing_passkey = passkey;
}

uint32_t ble_service_get_active_pairing_passkey(void)
{
    return _pairing_passkey;
}

bool ble_service_update_connection_parameters(
    uint16_t itvl_min,
    uint16_t itvl_max,
    uint16_t latency,
    uint16_t supervision_timeout,
    uint16_t min_ce_len,
    uint16_t max_ce_len)
{
    if(BLE_HS_CONN_HANDLE_NONE == ble_device_conn_handle)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, no active connection" NEW_LINE, __FUNCTION__);
        return false;
    }
    
    int status = BLE_HS_ENOERR;

    struct ble_gap_upd_params gap_params_to_apply = {0};
    gap_params_to_apply.itvl_min = itvl_min;
    gap_params_to_apply.itvl_max = itvl_max;
    gap_params_to_apply.latency = latency;
    gap_params_to_apply.supervision_timeout = supervision_timeout;
    gap_params_to_apply.min_ce_len = min_ce_len;
    gap_params_to_apply.max_ce_len = max_ce_len;

    if((status = ble_gap_update_params(ble_device_conn_handle, &gap_params_to_apply)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_update_params failed %s" NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    return true;
}

bool ble_service_request_mtu_exchange(void)
{
    if(BLE_HS_CONN_HANDLE_NONE == ble_device_conn_handle)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, no active connection" NEW_LINE, __FUNCTION__);
        return false;
    }

    int status = BLE_HS_ENOERR;

    if((status = ble_gattc_exchange_mtu(ble_device_conn_handle, &(ble_gatt_mtu_cb), NULL)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gattc_exchange_mtu %s" NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    return true;
}

bool ble_service_send_nus_data(const uint8_t *data, uint16_t length)
{
    // The NUS is TX is using notifications
    if(notification_data.transfer_in_progress)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, a transfer is already in progress"NEW_LINE, __FUNCTION__);
        return false;
    }

    notification_data.transfer_in_progress = true;
    notification_data.data = data;
    notification_data.length = length;

    return ble_service_send_custom_notification(gatt_nus_char_tx_handle, &notification_data);
}

void ble_service_register_nus_data_rx_cb(nus_data_rx_fn_t nus_data_rx_cb)
{
    _ble_service_nus_data_rx_cb = nus_data_rx_cb;
}

void ble_service_set_battery_value(uint8_t value)
{
    _battery_level_value = value > 100 ? 100 : value;
}

/**
 * PRIVATE FUNCTION DEFINITION
 * Used for the internal workings of the service
 */

static bool ble_service_send_raw_custom_notification(uint16_t characteristic_handle, const uint8_t *data, uint16_t length)
{
    if(BLE_HS_CONN_HANDLE_NONE == ble_device_conn_handle)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, no active connection" NEW_LINE, __FUNCTION__);
        return false;
    }

    if(!data || !length)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, no data" NEW_LINE, __FUNCTION__);
        return false;
    }

    int status = BLE_HS_ENOERR;
    struct os_mbuf *om_buf = ble_hs_mbuf_from_flat(data, length);

    if(!om_buf)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_hs_mbuf_from_flat" NEW_LINE, __FUNCTION__);
        return false;
    }

    if((status = ble_gattc_notify_custom(ble_device_conn_handle, characteristic_handle, om_buf)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_hs_mbuf_from_flat %s" NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    return true;
}

static bool ble_service_send_custom_notification(uint16_t characteristic_handle, data_being_sent_t * const notif_data)
{
    if(!notif_data)
    {
        TLS_BT_APPL_TRACE_WARNING("%s, notif_data is NULL"NEW_LINE, __FUNCTION__);
        return false;
    }

    // We compute the maximum size of the data we can send:
    uint16_t remaining_data_to_send = notif_data->length - notif_data->offset;
    notif_data->sent_chunk_size = remaining_data_to_send <= usable_mtu ? remaining_data_to_send : usable_mtu;

    if(!ble_service_send_raw_custom_notification(characteristic_handle, &notif_data->data[notif_data->offset], notif_data->sent_chunk_size))
    {
        //Transfer failed :
        reset_data_being_sent(notif_data);
        return false;
    }

    return true;
}

int ble_gatt_mtu_cb(uint16_t conn_handle, const struct ble_gatt_error *error, uint16_t mtu, void *arg)
{
    TLS_BT_APPL_TRACE_DEBUG("ble_gatt_mtu_cb"NEW_LINE);

    switch(error->status)
    {
        case 0:
            TLS_BT_APPL_TRACE_DEBUG("mtu exchange complete: conn_handle=%d mtu=%d"NEW_LINE,
                                    conn_handle, mtu);
            usable_mtu = mtu - 3;
            break;
        default:
            TLS_BT_APPL_TRACE_ERROR("Update MTU failed...error->status=%d"NEW_LINE, error->status);
            break;
    }

    return BLE_HS_ENOERR;
}

static bool ble_service_define_gatt(const struct ble_gatt_svc_def *gatt_svc)
{
    int status = BLE_HS_ENOERR;

    if((status = ble_gatts_count_cfg(gatt_svc)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gatts_count_cfg failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    if((status = ble_gatts_add_svcs(gatt_svc)) != BLE_HS_ENOERR)
    {
        TLS_BT_APPL_TRACE_ERROR("%s, ble_gatts_add_svcs failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
        return false;
    }

    return status == BLE_HS_ENOERR ? true : false;
}

static bool ble_service_advertise(bool enable)
{
    int status = BLE_HS_ENOERR;

    if(enable)
    {
        if(!ble_modem_is_sleeping())ble_modem_sleep();

        struct ble_hs_adv_fields advertisement_fields = {0};
        uint8_t own_addr_type = BLE_OWN_ADDR_RANDOM;
        
        advertisement_fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
        
        advertisement_fields.appearance = ble_svc_gap_device_appearance();
        advertisement_fields.appearance_is_present = 1;

        // Set the name of the BLE device
        advertisement_fields.name = (uint8_t *)ble_svc_gap_device_name();
        advertisement_fields.name_len = strlen(ble_svc_gap_device_name());

        advertisement_fields.name_is_complete = 1;

        advertisement_fields.uuids16 = (ble_uuid16_t[])
        {
            BLE_UUID16_INIT(BLE_DEVICE_ADV_SERVICE)
        };
        advertisement_fields.num_uuids16 = 1;
        advertisement_fields.uuids16_is_complete = 1;

        // Lets apply the advertisement data
        if((status = ble_gap_adv_set_fields(&advertisement_fields)) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_adv_set_fields failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            return false;
        }

        // We set the device address
        const uint8_t *device_mac_address = ble_service_get_device_mac_address();

        if((status = ble_hs_id_set_rnd(device_mac_address)) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_hs_id_set_rnd failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            return false;
        }
        else
        {
            TLS_BT_APPL_TRACE_VERBOSE("addr type : %s"NEW_LINE"device addr : %02X:%02X:%02X:%02X:%02X:%02X"NEW_LINE,
            tls_bt_addr_type_2_str(own_addr_type),
            device_mac_address[5],
            device_mac_address[4],
            device_mac_address[3],
            device_mac_address[2],
            device_mac_address[1],
            device_mac_address[0]);
        }

        // We are now ready to configure the advertisement parameters
        struct ble_gap_adv_params advertisement_params = {0};
        
        advertisement_params.conn_mode = BLE_GAP_CONN_MODE_UND;
        advertisement_params.disc_mode = BLE_GAP_DISC_MODE_GEN;

        advertisement_params.itvl_min = 160;
        advertisement_params.itvl_max = 160; //160 / 0.625 = 100 ms
        advertisement_params.filter_policy = 0;
        advertisement_params.channel_map = 0; // Use sane default
        advertisement_params.high_duty_cycle = 0;

        if((status = ble_gap_adv_start(BLE_OWN_ADDR_RANDOM, NULL, BLE_HS_FOREVER, &advertisement_params, &(ble_advertise_gap_event_cb), NULL)) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_adv_start failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            return false;
        }
    }
    else
    {
        if((status = ble_gap_adv_stop()) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_adv_stop failed : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            return false;
        }
    }

    return true;
}


// This is the brain of the ble service, here we handle all the possible GAP events
static int ble_gap_event_cb(struct ble_gap_event *event, void *arg)
{
    int status = BLE_HS_ENOERR;
    struct ble_gap_conn_desc desc;
    bool error = false;

    TLS_BT_APPL_TRACE_EVENT("ble_gap_event_cb : %s"NEW_LINE, tls_bt_gap_evt_2_str(event->type));

    switch(event->type)
    {
        case BLE_GAP_EVENT_CONNECT:
            if(event->connect.status == BLE_HS_ENOERR)
            {
                if((status = ble_gap_conn_find(event->connect.conn_handle, &desc)) == BLE_HS_ENOERR)
                {
                    print_conn_desc(&desc);
                    if(desc.role == BLE_GAP_ROLE_SLAVE)
                    {
                        _ble_service_state = BLE_SERVICE_MODE_CONNECTED;
                        usable_mtu = USABLE_DEFAULT_MTU;
                        ble_device_conn_handle = event->connect.conn_handle;

                        // We register an event callback on the connection,
                        // this allows to be notified on the passkey action event
                        if((status = ble_gap_set_event_cb(event->connect.conn_handle, &(ble_conn_gap_event_cb), NULL)) != BLE_HS_ENOERR)
                        {
                            TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_set_event_cb %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                        }
                        //We call the state change callback if registered
                        if(_ble_service_state_change_cb)_ble_service_state_change_cb(_ble_service_state);

                        if(ble_modem_is_sleeping())ble_modem_wake();
                    }
                    else
                    {
                        error = true;
                        if((status = ble_gap_terminate(event->connect.conn_handle, BLE_ERR_REM_USER_CONN_TERM)) != BLE_HS_ENOERR)
                        {
                            TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_terminate %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                        }
                    }
                }
            }
            else
            {
                error = true;
            }

            //Resume advertising :
            if(error)
            {
                TLS_BT_APPL_TRACE_VERBOSE("Remote device failed to connect, advertise again"NEW_LINE);
                if(!ble_service_advertise(true))
                {
                    _ble_service_state = BLE_SERVICE_MODE_IDLE;
                    return BLE_HS_EUNKNOWN;
                }
                _ble_service_state = BLE_SERVICE_MODE_ADVERTISING;
            }
            break;
        case BLE_GAP_EVENT_DISCONNECT:
            if(event->disconnect.conn.role != BLE_GAP_ROLE_SLAVE) return 0;

            TLS_BT_APPL_TRACE_DEBUG("Server disconnect reason=%d[0x%02x],state=%s"NEW_LINE, event->disconnect.reason,event->disconnect.reason-0x200,
                                    ble_service_state_2_str(_ble_service_state));

            // Don't forget to invalidate the connection handle :
            ble_device_conn_handle = BLE_HS_CONN_HANDLE_NONE;

            if(_ble_service_state == BLE_SERVICE_MODE_EXITING)
            {
                _ble_service_state = BLE_SERVICE_MODE_STOPPED;

                if((status = ble_gap_event_listener_unregister(&ble_gap_event_listener)) != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_event_listener_unregister %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                }

                // We finally clean the gatt registered services
                if((status = ble_gatts_reset()) != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_ERROR("%s, ble_gatts_reset %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                }
            }
            else
            {
                // Let's advertise again
                TLS_BT_APPL_TRACE_VERBOSE("Service disconnect event, advertise again"NEW_LINE);
                if(!ble_service_advertise(true))
                {
                    _ble_service_state = BLE_SERVICE_MODE_IDLE;
                    return BLE_HS_EUNKNOWN;
                }
                _ble_service_state = BLE_SERVICE_MODE_ADVERTISING;
            }

            //We call the state change callback if registered
            if(_ble_service_state_change_cb)_ble_service_state_change_cb(_ble_service_state);
            break;
        case BLE_GAP_EVENT_CONN_UPDATE:
            TLS_BT_APPL_TRACE_DEBUG("Conn update status : %s"NEW_LINE, tls_bt_rc_2_str(event->conn_update.status));
            if((status = ble_gap_conn_find(event->connect.conn_handle, &desc)) == BLE_HS_ENOERR)
            {
               print_conn_desc(&desc);
               TLS_BT_APPL_TRACE_VERBOSE("ble_gap_security_initiate %s"NEW_LINE, tls_bt_rc_2_str(ble_gap_security_initiate(ble_device_conn_handle)));
            }
            else
            {
                TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_conn_find %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            }
            break;
        case BLE_GAP_EVENT_SUBSCRIBE:
            TLS_BT_APPL_TRACE_VERBOSE("Attr handle : %u"NEW_LINE
            "cur_indicate : %u"NEW_LINE
            "prev_indicate : %u"NEW_LINE
            "cur_notify : %u"NEW_LINE
            "prev_notify : %u"NEW_LINE,
            event->subscribe.attr_handle,
            event->subscribe.cur_indicate,
            event->subscribe.prev_indicate,
            event->subscribe.cur_notify,
            event->subscribe.prev_notify
            );
            
            if(gatt_nus_char_tx_handle == event->subscribe.attr_handle)
            {
                if(event->subscribe.cur_notify || event->subscribe.cur_indicate)
                {
                    //We call the state change callback if registered
                    if(_ble_service_state_change_cb)_ble_service_state_change_cb(BLE_SERVICE_MODE_SUBSCRIBED);
                }
                else if(!event->subscribe.cur_notify && !event->subscribe.cur_indicate)
                {
                    //We call the state change callback if registered
                    if(_ble_service_state_change_cb)_ble_service_state_change_cb(BLE_SERVICE_MODE_UNSUBSCRIBED);
                }
            }
            break;
        case BLE_GAP_EVENT_MTU:
            TLS_BT_APPL_TRACE_VERBOSE("MTU update : %u"NEW_LINE, event->mtu.value);
            usable_mtu = event->mtu.value - 3;
            break;
        case BLE_GAP_EVENT_NOTIFY_TX:
            if(event->notify_tx.indication == 0) // Notification
            {   
                TLS_BT_APPL_TRACE_VERBOSE("Type : notification"NEW_LINE);
                if(event->notify_tx.status != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_WARNING("%s, notify_tx notification error %d, transfer in progress : %u"NEW_LINE, __FUNCTION__, event->notify_tx.status, notification_data.transfer_in_progress);
                    reset_data_being_sent(&notification_data);
                    return BLE_HS_EUNKNOWN;
                }

                TLS_BT_APPL_TRACE_DEBUG("transfer : %u"NEW_LINE"length : %u"NEW_LINE"offset : %u"NEW_LINE, 
                notification_data.transfer_in_progress,
                notification_data.length,
                notification_data.offset);

                // We update the offset :
                notification_data.offset += notification_data.sent_chunk_size;
                
                if(notification_data.offset < notification_data.length) // Still data to send ?
                {
                    if(!ble_service_send_custom_notification(event->notify_tx.attr_handle, &notification_data))
                    {
                        TLS_BT_APPL_TRACE_ERROR("Failed to send next notification chunk, aborting"NEW_LINE);
                        reset_data_being_sent(&notification_data);
                        return BLE_HS_EUNKNOWN;
                    }
                }
                else
                {
                    TLS_BT_APPL_TRACE_VERBOSE("last data chunk sent, end of the transfer"NEW_LINE);
                    // All data have been sent, end of the transfer
                    reset_data_being_sent(&notification_data);
                }
            }
            else // Indication
            {
                TLS_BT_APPL_TRACE_WARNING("Indication not yet handled"NEW_LINE);
            }
            break;
        default:
            TLS_BT_APPL_TRACE_WARNING("unhandled event !"NEW_LINE);
    }

    return BLE_HS_ENOERR;
}

static int ble_advertise_gap_event_cb(struct ble_gap_event *event, void *arg)
{
    return BLE_HS_ENOERR;
}

static int ble_conn_gap_event_cb(struct ble_gap_event *event, void *arg)
{
    int status = BLE_HS_ENOERR;
    struct ble_gap_conn_desc desc;

    TLS_BT_APPL_TRACE_EVENT("%s : %s"NEW_LINE, __FUNCTION__, tls_bt_gap_evt_2_str(event->type));

    switch(event->type)
    {
        case BLE_GAP_EVENT_PASSKEY_ACTION:
            TLS_BT_APPL_TRACE_VERBOSE("conn_handle : %u"NEW_LINE
            "action : %s"NEW_LINE,
            event->passkey.conn_handle,
            tls_bt_sm_ioact_2_str(event->passkey.params.action));

            if(BLE_SM_IOACT_NUMCMP == event->passkey.params.action)
            {
                TLS_BT_APPL_TRACE_VERBOSE("numcmp : %u"NEW_LINE,
                event->passkey.params.numcmp);
            }

            struct ble_sm_io sm_io = {
                .action  = BLE_SM_IOACT_DISP,
                .passkey = _pairing_passkey,
            };

            if((status = ble_sm_inject_io(event->passkey.conn_handle, &sm_io)) != BLE_HS_ENOERR)
            {
                TLS_BT_APPL_TRACE_ERROR("%s, ble_sm_inject_io %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            }
            break;
        case BLE_GAP_EVENT_CONN_UPDATE:
            TLS_BT_APPL_TRACE_DEBUG("Conn update status : %s"NEW_LINE, tls_bt_rc_2_str(event->conn_update.status));
            if((status = ble_gap_conn_find(event->connect.conn_handle, &desc)) == BLE_HS_ENOERR)
            {
               print_conn_desc(&desc);
            }
            else
            {
                TLS_BT_APPL_TRACE_WARNING("%s, ble_gap_conn_find %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
            }
            break;
        case BLE_GAP_EVENT_ENC_CHANGE:
            TLS_BT_APPL_TRACE_DEBUG("Enc change status : (%u) -> %s"NEW_LINE, event->enc_change.status, tls_bt_rc_2_str(event->enc_change.status));
            break;
        case BLE_GAP_EVENT_REPEAT_PAIRING:
        {
            // If this event is triggered, then we shall remove the device from the bonded device list
            // and tell it to pair again.
            uint16_t conn_handle = event->repeat_pairing.conn_handle;
            TLS_BT_APPL_TRACE_VERBOSE("conn_handle : %u"NEW_LINE
            "cur_key_size : %u"NEW_LINE
            "cur_authenticated : %u"NEW_LINE
            "cur_sc : %u"NEW_LINE
            "new_key_size : %u"NEW_LINE
            "new_authenticated : %u"NEW_LINE
            "new_sc : %u"NEW_LINE
            "new_bonding : %u"NEW_LINE,
            event->repeat_pairing.conn_handle,
            event->repeat_pairing.cur_key_size,
            event->repeat_pairing.cur_authenticated,
            event->repeat_pairing.cur_sc,
            event->repeat_pairing.new_key_size,
            event->repeat_pairing.new_authenticated,
            event->repeat_pairing.new_sc,
            event->repeat_pairing.new_bonding);
            
            if((status = ble_gap_conn_find(conn_handle, &desc)) != BLE_HS_ENOERR)
            {
                TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_conn_find %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                return status;
            }

            TLS_BT_APPL_TRACE_VERBOSE("sec_state_encrypted : %u"NEW_LINE
            "sec_state_authenticated : %u"NEW_LINE
            "sec_state_bonded : %u"NEW_LINE
            "sec_state_key_size : %u"NEW_LINE
            "our_id_addr(%u) : %02X:%02X:%02X:%02X:%02X:%02X"NEW_LINE
            "our_ota_addr(%u) : %02X:%02X:%02X:%02X:%02X:%02X"NEW_LINE
            "peer_id_addr(%u) : %02X:%02X:%02X:%02X:%02X:%02X"NEW_LINE
            "peer_ota_addr(%u) : %02X:%02X:%02X:%02X:%02X:%02X"NEW_LINE,
            desc.sec_state.encrypted,
            desc.sec_state.authenticated,
            desc.sec_state.bonded,
            desc.sec_state.key_size,
            desc.our_id_addr.type,
            desc.our_id_addr.val[5],
            desc.our_id_addr.val[4],
            desc.our_id_addr.val[3],
            desc.our_id_addr.val[2],
            desc.our_id_addr.val[1],
            desc.our_id_addr.val[0],
            desc.our_ota_addr.type,
            desc.our_ota_addr.val[5],
            desc.our_ota_addr.val[4],
            desc.our_ota_addr.val[3],
            desc.our_ota_addr.val[2],
            desc.our_ota_addr.val[1],
            desc.our_ota_addr.val[0],
            desc.peer_id_addr.type,
            desc.peer_id_addr.val[5],
            desc.peer_id_addr.val[4],
            desc.peer_id_addr.val[3],
            desc.peer_id_addr.val[2],
            desc.peer_id_addr.val[1],
            desc.peer_id_addr.val[0],
            desc.peer_ota_addr.type,
            desc.peer_ota_addr.val[5],
            desc.peer_ota_addr.val[4],
            desc.peer_ota_addr.val[3],
            desc.peer_ota_addr.val[2],
            desc.peer_ota_addr.val[1],
            desc.peer_ota_addr.val[0]);

            if((status = ble_gap_unpair(&desc.peer_id_addr)) != BLE_HS_ENOERR)
            {
                TLS_BT_APPL_TRACE_ERROR("%s, ble_gap_unpair %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                return status;
            }

            return BLE_GAP_REPEAT_PAIRING_RETRY;
        }
            break;
        default:
            TLS_BT_APPL_TRACE_WARNING("unhandled event !"NEW_LINE);
    }

    return BLE_HS_ENOERR;
}

static int battery_level_char_access_cb(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    int status = BLE_HS_ENOERR;
    TLS_BT_APPL_TRACE_EVENT("battery_level_char_access_cb op : %s"NEW_LINE, tls_bt_access_opt_2_str(ctxt->op));
    switch(ctxt->op)
    {
        case BLE_GATT_ACCESS_OP_READ_CHR:
            if(attr_handle == battery_level_char_handle)
            {
                TLS_BT_APPL_TRACE_VERBOSE("battery level reading"NEW_LINE);
                if((status = os_mbuf_append(ctxt->om, &_battery_level_value, sizeof(_battery_level_value))) != BLE_HS_ENOERR)
                {
                    TLS_BT_APPL_TRACE_ERROR("%s, battery level os_mbuf : %s"NEW_LINE, __FUNCTION__, tls_bt_rc_2_str(status));
                }
            }
            break;
        default:
            TLS_BT_APPL_TRACE_WARNING("unhandled operation !"NEW_LINE);
    }

    return BLE_HS_ENOERR;
}

static int gatt_nus_char_access_cb(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    //TLS_BT_APPL_TRACE_EVENT("gatt_nus_char_access_cb op : %s"NEW_LINE, tls_bt_access_opt_2_str(ctxt->op));
    switch(ctxt->op)
    {
        case BLE_GATT_ACCESS_OP_WRITE_CHR:
        {
            if(ble_modem_is_sleeping())ble_modem_wake();
            struct os_mbuf *om_buf = ctxt->om;
            while(om_buf)
            {
                // Call the nus rx cb function if one is registered
                if(_ble_service_nus_data_rx_cb)
                    _ble_service_nus_data_rx_cb(om_buf->om_data, om_buf->om_len);

                om_buf = SLIST_NEXT(om_buf, om_next);
            }
        }
            break;
        default:
            TLS_BT_APPL_TRACE_WARNING("unhandled operation !"NEW_LINE);
    }

    return BLE_HS_ENOERR;
}

static void print_conn_desc(const struct ble_gap_conn_desc *desc)
{
    TLS_BT_APPL_TRACE_VERBOSE("conn_handle : %u"NEW_LINE"conn_itvl : %u"NEW_LINE"conn_latency : %u"NEW_LINE"master_clock_accuracy : %u"NEW_LINE"role : %s"NEW_LINE"supervision_timeout : %u"NEW_LINE
    "encrypted : %u"NEW_LINE"authenticated : %u"NEW_LINE"bonded : %u"NEW_LINE,
                desc->conn_handle,
                desc->conn_itvl,
                desc->conn_latency, 
                desc->master_clock_accuracy,
                desc->role == BLE_GAP_ROLE_MASTER ? "MASTER" : "SLAVE",
                desc->supervision_timeout,
                desc->sec_state.encrypted,
                desc->sec_state.authenticated,
                desc->sec_state.bonded);
}

static void reset_data_being_sent(data_being_sent_t * const data)
{
    if(data)
    {
        memset(data, 0, sizeof(data_being_sent_t));
    }
}