#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"

#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;

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

/**
 * @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
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;
static uint8_t battery_level_value = 42;


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
            },
            {
                .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 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;
}

ble_service_state_e ble_service_get_state(void)
{
    return _ble_service_state;
}

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_nus_send_data(const uint8_t *data, uint16_t length)
{
    // The NUS 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_nus_register_data_rx_cb(nus_data_rx_fn_t nus_data_rx_cb)
{
    _ble_service_nus_data_rx_cb = nus_data_rx_cb;
}
/**
 * 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)
    {
        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 a watch supported by GB        
        #ifdef GADGETBRIDGE_SUPPORT
        static const char dev_name[12] = "Bangle.js 2";
        advertisement_fields.name = (uint8_t *)dev_name;//ble_svc_gap_device_name();
        advertisement_fields.name_len = 11;//strlen(ble_svc_gap_device_name());
        #else
        advertisement_fields.name = (uint8_t *)ble_svc_gap_device_name();
        advertisement_fields.name_len = strlen(ble_svc_gap_device_name());
        #endif

        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 the device address
        uint8_t device_addr[6] = {0};
        extern int tls_get_bt_mac_addr(u8 *mac);
        tls_get_bt_mac_addr(device_addr);

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

        if((status = ble_hs_id_set_rnd(device_addr)) != 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_addr[5],
            device_addr[4],
            device_addr[3],
            device_addr[2],
            device_addr[1],
            device_addr[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;
                    }
                    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;
            }
            break;
        case BLE_GAP_EVENT_CONN_UPDATE:
            TLS_BT_APPL_TRACE_DEBUG("Conn update status : %d"NEW_LINE, 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_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
            );
            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 has 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 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:
        {
            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));
    }
}