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


static volatile ble_service_state_t ble_service_state = BLE_SERVICE_MODE_STOPPED;
/* Connection handle to the connected device : only one simultaneous connection */
static uint16_t ble_device_conn_handle = BLE_HS_CONN_HANDLE_NONE;
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);
// 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);

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"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;
    }

    // 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_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_send_custom_notification(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, gatt_nus_char_tx_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;
}

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

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);
            break;
        default:
            TLS_BT_APPL_TRACE_ERROR("Update MTU failed...error->status=%d"NEW_LINE, error->status);
            break;
    }

    return BLE_HS_ENOERR;
}

const char *ble_service_state_2_str(uint8_t 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";
    }
}

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

        if((status = ble_hs_id_set_rnd(device_addr)) != BLE_HS_ENOERR)
        {
            TLS_BT_APPL_TRACE_ERROR("%s, ble_hs_id_infer_auto 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;
                        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);
            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;
            TLS_BT_APPL_TRACE_VERBOSE("Received data : "NEW_LINE);
            while(om_buf)
            {
                for(uint16_t i = 0; i < om_buf->om_len; i++)
                {
                    if(om_buf->om_data[i] < 32)
                        printf("[%u]", om_buf->om_data[i]);
                    else
                        putchar(om_buf->om_data[i]);
                }
                om_buf = SLIST_NEXT(om_buf, om_next);
            }
            printf(NEW_LINE);
        }
            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);
}