void CSensorBluetoothWrapper::WriteGattChar(void * _pvContext,
                                            const void * _pvCaller,
                                            MblMwGattCharWriteType _eWriteType,
                                            const MblMwGattChar * _pstCharacteristic,
                                            const uint8_t * _pubValue,
                                            uint8_t _pubLength)
{
    // default
    bool doGattWrite = false;

    CSensorBluetoothWrapper* wrapper = static_cast<CSensorBluetoothWrapper*>(_pvContext);
    std::string uuid = uuidConvertToString(_pstCharacteristic->uuid_high, _pstCharacteristic->uuid_low);
    // replace with platform specific BluetoothGatt code
    WarbleGattChar * gatt_char = wrapper->m_poWarble->find_characteristic(uuid.c_str());

    {
        ::std::unique_lock<::std::mutex> oGatWriteLock(wrapper->m_oGattWriteMutex);

        // update
        doGattWrite = wrapper->write_queue.empty();

        // add gatt to write
        WriteElement * const poWriteElement = new WriteElement(gatt_char,
                                                               _pubValue,
                                                               _pubLength,
                                                               _eWriteType);
        wrapper->write_queue.emplace_back(poWriteElement);
    }

    // if write has to be done
    if (true == doGattWrite)
    {
        // do gatt write
        wrapper->WriteGattCharAsync();
    }
}

voidCSensorBluetoothWrapper::WriteGattCharAsync()
{
    // default
    WriteElement * poWriteElement = nullptr;

    m_oGattWriteMutex.lock();

    bool const doGattWrite = !write_queue.empty();
    // if some write to do
    if (true == doGattWrite)
    {
        // get gatt to write
        poWriteElement = this->write_queue.front();
    }

    m_oGattWriteMutex.unlock();

    if (nullptr != poWriteElement)
    {
        auto completed = [] (void* context, WarbleGattChar* caller, const char* value) {
            assert(context!=nullptr);
            CSensorBluetoothWrapper * const poSensorBluetoothWrapper
                = static_cast<CSensorBluetoothWrapper *>(context);
            if(nullptr != value)
            {
                std::cerr << "ERROR CSensorBluetoothWrapper::WriteGattCharAsync-->completed" << value << std::endl;
            }

            poSensorBluetoothWrapper->m_oGattWriteMutex.lock();

            // get the written gatt
            WriteElement * const poWrittenElement = poSensorBluetoothWrapper->write_queue.front();
            poSensorBluetoothWrapper->write_queue.pop_front();
            bool const doGattWrite = !(poSensorBluetoothWrapper->write_queue.empty());

            poSensorBluetoothWrapper->m_oGattWriteMutex.unlock();

            delete poWrittenElement;

            if (true == doGattWrite)
            {
                poSensorBluetoothWrapper->WriteGattCharAsync();
            }
        };

        if (   poWriteElement->getWriteType()
            ==  MblMwGattCharWriteType::MBL_MW_GATT_CHAR_WRITE_WITH_RESPONSE)
        {
            warble_gattchar_write_async(poWriteElement->getGattChar(),
                                        poWriteElement->getValue().data(),
                                        poWriteElement->getValue().size(),
                                        this,
                                        completed);
        }
        else
        {
            warble_gattchar_write_without_resp_async(poWriteElement->getGattChar(),
                                                     poWriteElement->getValue().data(),
                                                     poWriteElement->getValue().size(),
                                                     this,
                                                     completed);
        }
    }
}

void CSensorBluetoothWrapper::ReadGattChar(void * _pvContext,
                                           const void * _pvCaller,
                                           const MblMwGattChar * _pstCharacteristic,
                                           MblMwFnIntVoidPtrArray _pfnHandler)
{
    CSensorBluetoothWrapper* wrapper = static_cast<CSensorBluetoothWrapper*>(_pvContext);
    // replace with platform specific BluetoothGatt code
    std::string uuid = uuidConvertToString(_pstCharacteristic->uuid_high, _pstCharacteristic->uuid_low);
    WarbleGattChar * gatt_char = wrapper->m_poWarble->find_characteristic(uuid.c_str());

    auto completed = [] (void* context, WarbleGattChar* caller, const WARBLE_UBYTE* value, WARBLE_UBYTE length, const char* value2) {
        assert(context!=nullptr);
        ContextRead* contextRead = static_cast<ContextRead*>(context);
        if(value!=nullptr)
        {
            contextRead->m_pfnHandler(contextRead->m_pvCaller, value, length);
        }
        delete contextRead;
    };

    warble_gattchar_read_async(
        gatt_char,
        new ContextRead(_pvCaller, *_pstCharacteristic, _pfnHandler),
        completed);

}

void CSensorBluetoothWrapper::EnableNotifications(void * _pvContext,
                                                  const void* _pvCaller,
                                                  const MblMwGattChar* _pstCharacteristic,
                                                  MblMwFnIntVoidPtrArray _pfnHandler,
                                                  MblMwFnVoidVoidPtrInt _pfnReady)
{
    CSensorBluetoothWrapper* wrapper = static_cast<CSensorBluetoothWrapper*>(_pvContext);
    // call the 'ready' function pointer when the enable notification request has finished
    std::string uuid = uuidConvertToString(_pstCharacteristic->uuid_high, _pstCharacteristic->uuid_low);
    WarbleGattChar * gatt_char = wrapper->m_poWarble->find_characteristic(uuid.c_str());

    if(gatt_char != nullptr)
    {
        auto completed = [] (void* context, WarbleGattChar* caller, const char* value) {
            assert(context!=nullptr);
            ContextNotification* contextNotification = static_cast<ContextNotification*>(context);
            if(value!=nullptr)
            {
                std::cerr << "ERROR CSensorBluetoothWrapper::EnableNotifications-->completed MBL_MW_STATUS_ERROR_ENABLE_NOTIFY" << std::endl;
                contextNotification->m_pfnReady(contextNotification->m_pvCaller, MBL_MW_STATUS_ERROR_ENABLE_NOTIFY);
            }
            else
            {
                warble_gattchar_on_notification_received(contextNotification->m_gatt_char, contextNotification,
                    [](void* context, WarbleGattChar* caller, const WARBLE_UBYTE* value, WARBLE_UBYTE length){
                        ContextNotification* contextNotification = static_cast<ContextNotification*>(context);
                        contextNotification->m_pfnHandler(contextNotification->m_pvCaller, value, length);
                });
                contextNotification->m_pfnReady(contextNotification->m_pvCaller, MBL_MW_STATUS_OK);
            }
            delete contextNotification;
        };

        warble_gattchar_enable_notifications_async(
            gatt_char,
            new ContextNotification(gatt_char, _pvCaller, *_pstCharacteristic, _pfnHandler, _pfnReady),
            completed);
    }
    else
    {
        std::cerr << "ERROR CSensorBluetoothWrapper::EnableNotifications MBL_MW_STATUS_ERROR_ENABLE_NOTIFY" << std::endl;
        _pfnReady(_pvCaller, MBL_MW_STATUS_ERROR_ENABLE_NOTIFY);
    }
}

void CSensorBluetoothWrapper::OnDisconnect(void * _pvContext,
                                           const void* _pvCaller,
                                           MblMwFnVoidVoidPtrInt _pfnHandler)
{
    CSensorBluetoothWrapper* wrapper = static_cast<CSensorBluetoothWrapper*>(_pvContext);

    auto completed = [] (void* context, WarbleGatt* caller, WARBLE_INT value) {
        assert(context!=nullptr);

        td_stDisconnectHandlerContext * const pstDisconnectHandlerContext
            = static_cast<td_stDisconnectHandlerContext*>(context);
        CSensorBluetoothWrapper * const poSensorBluetoothWrapper = pstDisconnectHandlerContext->poSensorBluetoothWrapper;

        // default
        bool isDisconnecting = false;
        bool isConnecting = false;

        {
            // lock management
            ::std::unique_lock<::std::mutex> oManagementLock(poSensorBluetoothWrapper->m_oWrapperManagementMutex);

            // lock access
            poSensorBluetoothWrapper->m_oWrapperStateMutex.lock();

            // get disconnection validity
            isDisconnecting = poSensorBluetoothWrapper->m_isDisconnecting;
            // reset
            poSensorBluetoothWrapper->m_isDisconnecting = false;

            // if connecting
            isConnecting = poSensorBluetoothWrapper->m_isConnecting;
            if (false == isConnecting)
            {
                // reset
                poSensorBluetoothWrapper->m_isConnected = false;
            }

            // unlock access
            poSensorBluetoothWrapper->m_oWrapperStateMutex.unlock();

            // check
            if (nullptr != poSensorBluetoothWrapper->m_poSensor)
            {
                // check
                if (false == isConnecting)
                {
                    // if unwanted disconnection
                    if (false == isDisconnecting)
                    {
                        // trace error
                        M_TRACE_ARGS(NSS_Utils::ETraceLevel::ETRACELEVEL_ERROR,
                                     NSS_UI::CTextConstants::ms_pszSensorDisconnected,
                                     poSensorBluetoothWrapper->m_oSensorMacAddressString.c_str());
                    }

                    // notify disconnect to sensor instance
                    pstDisconnectHandlerContext->pfnHandler(pstDisconnectHandlerContext->pvCaller,
                                                            MBL_MW_STATUS_OK);

                    // if wanted disconnection
                    if (true == isDisconnecting)
                    {
                        // notify disconnection end
                        poSensorBluetoothWrapper->m_oDisconnectionSignal.notify_one();
                    }
                }
                else
                {
                    // notify disconnect to sensor instance
                    pstDisconnectHandlerContext->pfnHandler(pstDisconnectHandlerContext->pvCaller,
                                                            MBL_MW_STATUS_OK);

                    // update
                    poSensorBluetoothWrapper->m_stConnectContext.ManageConnectionEnd(false);

                }
            }
        }
    };

    // update
    wrapper->m_stDisconnectHandlerContext.pvCaller = _pvCaller;
    wrapper->m_stDisconnectHandlerContext.pfnHandler = _pfnHandler;

    // initialize
    warble_gatt_on_disconnect(wrapper->m_poWarble,
                              &(wrapper->m_stDisconnectHandlerContext),
                              completed);
}

CSensorBluetoothWrapper::CreateWarble()
{
    // create warble options
    const ::WarbleOption opts[] = {{"mac", m_oSensorMacAddressString.c_str()},
                                   {"hci", m_oControllerMacAddressString.c_str()}};
    // create warble instance
    m_poWarble = warble_gatt_create_with_options(sizeof(opts)/sizeof(WarbleOption), opts);

    //using namespace std::chrono_literals;
    //std::this_thread::sleep_for(1s);

    MblMwBtleConnection interface;
    // fill interface
    interface.context = static_cast<void *>(this);
    interface.write_gatt_char = CSensorBluetoothWrapper::WriteGattChar;
    interface.read_gatt_char = CSensorBluetoothWrapper::ReadGattChar;
    interface.enable_notifications = CSensorBluetoothWrapper::EnableNotifications;
    interface.on_disconnect = CSensorBluetoothWrapper::OnDisconnect;

    m_poSensor = mbl_mw_metawearboard_create(&interface);
}