/**
 * Copyright (c) 2018 - 2020, Nordic Semiconductor ASA
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 *
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "sdk_common.h"
#if NRF_MODULE_ENABLED(NRF_BLE_GQ)

#include "nrf_ble_gq.h"

#define NRF_LOG_MODULE_NAME nrf_ble_gq
#include "nrf_log.h"
NRF_LOG_MODULE_REGISTER();

/**@brief Pointer used to describe memory allocator for GATT request. */
typedef ret_code_t (* req_data_alloc_t) (nrf_memobj_pool_t const * p_data_pool, 
                                         nrf_ble_gq_req_t  * const p_req);


/**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTC_WRITE
 *        request.
 *
 * @param[in] p_data_pool  Pointer to general memory pool.
 * @param[in] p_req        Pointer to GATTC write request.
 *
 * @retval    NRF_SUCCESS              If the write data was allocated successfully.
 * @retval    NRF_ERROR_INVALID_LENGTH If data to be written is too long.
 * @retval    NRF_ERROR_NO_MEM         If there was no room either in the data pool for new allocation.
 */
static ret_code_t gattc_write_alloc(nrf_memobj_pool_t const * p_data_pool,
                                    nrf_ble_gq_req_t  * const p_req)
{
    nrf_ble_gq_gattc_write_t * p_gattc_write = &p_req->params.gattc_write;

    // Check if the payload data is not too long.
    if (p_gattc_write->len > NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN)
    {
        return NRF_ERROR_INVALID_LENGTH;
    }

    // Allocate memory for GATTC write request.
    p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
                                        p_gattc_write->len);
    if (p_req->p_mem_obj == NULL)
    {
        return NRF_ERROR_NO_MEM;
    }

    // Copy relevant data to the pool.
    nrf_memobj_write(p_req->p_mem_obj, (void *) p_gattc_write->p_value, p_gattc_write->len, 0);

    NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
    return NRF_SUCCESS;
}


/**@brief Function allocates memory for data associated with @ref NRF_BLE_GQ_REQ_GATTS_HVX
 *        request.
 *
 * @param[in] p_data_pool  Pointer to general memory pool.
 * @param[in] p_req        Pointer to GATTS hvx request.
 *
 * @retval    NRF_SUCCESS              If the notification or indication data was allocated successfully.
 * @retval    NRF_ERROR_INVALID_LENGTH If data to be written is too long.
 * @retval    NRF_ERROR_NO_MEM         If there was no room either in the data pool for new allocation.
 */
static ret_code_t gatts_hvx_alloc(nrf_memobj_pool_t const * p_data_pool,
                                  nrf_ble_gq_req_t  * const p_req)
{
    nrf_ble_gq_gatts_hvx_t * p_gatts_hvx = &p_req->params.gatts_hvx;

    // Check if the payload data is not too long.
    if (*p_gatts_hvx->p_len > NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN)
    {
        return NRF_ERROR_INVALID_LENGTH;
    }

    // Allocate memory for GATTS notification or indication request.
    p_req->p_mem_obj = nrf_memobj_alloc(p_data_pool,
                                        *p_gatts_hvx->p_len + sizeof(uint16_t));
    if (p_req->p_mem_obj == NULL)
    {
        return NRF_ERROR_NO_MEM;
    }

    // Copy relevant data to the pool.
    nrf_memobj_write(p_req->p_mem_obj, (void *)p_gatts_hvx->p_len, sizeof(uint16_t), 0);
    nrf_memobj_write(p_req->p_mem_obj, 
                     (void *)p_gatts_hvx->p_data, 
                     *p_gatts_hvx->p_len, 
                     sizeof(uint16_t));

    NRF_LOG_DEBUG("Pointer to allocated memory block: %p.", p_req->p_mem_obj);
    return NRF_SUCCESS;
}


/**@brief Array of memory allocators for different types of @ref nrf_ble_gq_req_t. */
static const req_data_alloc_t m_req_data_alloc[NRF_BLE_GQ_REQ_NUM] =
{
    [NRF_BLE_GQ_REQ_GATTC_READ]     = NULL,
    [NRF_BLE_GQ_REQ_GATTC_WRITE]    = gattc_write_alloc,
    [NRF_BLE_GQ_REQ_SRV_DISCOVERY]  = NULL,
    [NRF_BLE_GQ_REQ_CHAR_DISCOVERY] = NULL,
    [NRF_BLE_GQ_REQ_DESC_DISCOVERY] = NULL,
    [NRF_BLE_GQ_REQ_GATTS_HVX]      = gatts_hvx_alloc
};


/**@brief Function handles error codes returned by GATT requests.
 *
 * @param[in] p_req       Pointer to GATT request.
 * @param[in] err_code    Error code returned by SoftDevice.
 * @param[in] conn_handle Connection handle.
 */
__STATIC_INLINE void request_err_code_handle(nrf_ble_gq_req_t const * const p_req,
                                             uint16_t                       conn_handle,
                                             ret_code_t                     err_code)
{
    if (err_code == NRF_SUCCESS)
    {
        NRF_LOG_DEBUG("SD GATT procedure (%d) succeeded on connection handle: %d.",
                       p_req->type,
                       conn_handle);
    }
    else
    {
        NRF_LOG_ERROR("SD GATT procedure (%d) failed on connection handle %d with error: 0x%08X.",
                      p_req->type, conn_handle, err_code);
        if (p_req->error_handler.cb != NULL)
        {
             p_req->error_handler.cb(err_code, p_req->error_handler.p_ctx, conn_handle);
        }
    }
}


/**@brief Function processes subsequent requests from the BGQ instance queue.
 *
 * @param[in] p_queue      Pointer to the queue instance.
 * @param[in] conn_handle  Connection handle.
 */
static void queue_process(nrf_queue_t const * const p_queue, uint16_t conn_handle)
{
    ret_code_t       err_code;
    nrf_ble_gq_req_t ble_req;

    NRF_LOG_DEBUG("Processing the request queue...");

    err_code = nrf_queue_peek(p_queue, &ble_req);
    if (err_code == NRF_SUCCESS) // Queue is not empty
    {
        switch (ble_req.type)
        {
            case NRF_BLE_GQ_REQ_GATTC_READ:
                NRF_LOG_DEBUG("GATTC Read Request");
                err_code = sd_ble_gattc_read(conn_handle,
                                             ble_req.params.gattc_read.handle,
                                             ble_req.params.gattc_read.offset);
                break;

            case NRF_BLE_GQ_REQ_GATTC_WRITE:
            {
                uint8_t write_data[NRF_BLE_GQ_GATTC_WRITE_MAX_DATA_LEN];

                // Retrieve allocated data.
                ble_req.params.gattc_write.p_value = write_data;
                nrf_memobj_read(ble_req.p_mem_obj,
                                (void *) ble_req.params.gattc_write.p_value,
                                ble_req.params.gattc_write.len, 0);

                NRF_LOG_DEBUG("GATTC Write Request");
                err_code = sd_ble_gattc_write(conn_handle,
                                              &ble_req.params.gattc_write);
            } break;

            case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
            {
                NRF_LOG_DEBUG("GATTC Primary Service Discovery Request");
                err_code = sd_ble_gattc_primary_services_discover(conn_handle,
                                                                  ble_req.params.gattc_srv_disc.start_handle,
                                                                  &ble_req.params.gattc_srv_disc.srvc_uuid);
            } break;

            case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
            {
                NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
                err_code = sd_ble_gattc_characteristics_discover(conn_handle,
                                                                 &ble_req.params.gattc_char_disc);
            } break;

            case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
            {
                NRF_LOG_DEBUG("GATTC Characteristic Descriptor Discovery Request")
                err_code = sd_ble_gattc_descriptors_discover(conn_handle,
                                                             &ble_req.params.gattc_desc_disc);
            } break;

            case NRF_BLE_GQ_REQ_GATTS_HVX:
            {
                uint8_t  hvx_data[NRF_BLE_GQ_GATTS_HVX_MAX_DATA_LEN];
                uint16_t len;
                uint16_t hvx_len;

                // Retrieve allocated data.
                ble_req.params.gatts_hvx.p_data = hvx_data;
                nrf_memobj_read(ble_req.p_mem_obj,
                                (void *) &hvx_len,
                                sizeof(uint16_t),
                                0);
                ble_req.params.gatts_hvx.p_len = &hvx_len;
                nrf_memobj_read(ble_req.p_mem_obj,
                                (void *) ble_req.params.gatts_hvx.p_data,
                                *ble_req.params.gatts_hvx.p_len,
                                sizeof(uint16_t));

                len = hvx_len;

                NRF_LOG_DEBUG("GATTS HVX");
                err_code = sd_ble_gatts_hvx(conn_handle,
                                            &ble_req.params.gatts_hvx);

                if ((err_code == NRF_SUCCESS) &&
                    (len != hvx_len))
                {
                    err_code = NRF_ERROR_DATA_SIZE;
                }
            } break;

            default:
                NRF_LOG_WARNING("Unimplemented GATT Request");
                break;
        }

        if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
        {
            NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
                          again later.");
        }
        else
        {
            // Remove last request descriptor from the queue and free data associated with it.
            if (m_req_data_alloc[ble_req.type] != NULL)
            {
                nrf_memobj_free(ble_req.p_mem_obj);
                NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
            }
            UNUSED_RETURN_VALUE(nrf_queue_pop(p_queue, &ble_req));

            request_err_code_handle(&ble_req, conn_handle, err_code);
        }
    }
}


/**@brief Function purges all requests from BGQ instance queues that are
 *        no longer used by any connection.
 *
 * @param[in] p_gatt_queue Pointer to the BGQ instance.
 */
static void queues_purge(nrf_ble_gq_t const * const p_gatt_queue)
{
    ret_code_t err_code;
    uint16_t   conn_id;

    err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);

    while (err_code == NRF_SUCCESS)
    {
        nrf_ble_gq_req_t    ble_req;
        nrf_queue_t const * p_queue;

        NRF_LOG_DEBUG("Purging request queue with id: %d", conn_id);

        p_queue  = &p_gatt_queue->p_req_queue[conn_id];
        err_code = nrf_queue_pop(p_queue, &ble_req);

        while (err_code == NRF_SUCCESS)
        {
            // Free data associated with this request if there is any.
            if (m_req_data_alloc[ble_req.type] != NULL)
            {
                nrf_memobj_free(ble_req.p_mem_obj);
                NRF_LOG_DEBUG("Pointer to freed memory block: %p.", ble_req.p_mem_obj);
            }

            err_code = nrf_queue_pop(p_queue, &ble_req);
        }

        err_code = nrf_queue_pop(p_gatt_queue->p_purge_queue, &conn_id);
    }
}


/**@brief Function processes single GATT request without queue.
 *
 * @param[in] p_req        Pointer to GATT request.
 * @param[in] conn_handle  Connection handle.
 *
 * @retval  true   If request is accepted by Softdevice.
 * @retval  false  If Softdevice is busy and the request should be queued.
 */
static bool request_process(nrf_ble_gq_req_t const * const p_req, uint16_t conn_handle)
{
    ret_code_t err_code = NRF_SUCCESS;

    switch (p_req->type)
    {
        case NRF_BLE_GQ_REQ_GATTC_READ:
            NRF_LOG_DEBUG("GATTC Read Request");
            err_code = sd_ble_gattc_read(conn_handle,
                                         p_req->params.gattc_read.handle,
                                         p_req->params.gattc_read.offset);
            break;

        case NRF_BLE_GQ_REQ_GATTC_WRITE:
            NRF_LOG_DEBUG("GATTC Write Request");
            err_code = sd_ble_gattc_write(conn_handle,
                                          &p_req->params.gattc_write);
            break;

        case NRF_BLE_GQ_REQ_SRV_DISCOVERY:
            NRF_LOG_DEBUG("GATTC Primary Services Discovery Request");
            err_code = sd_ble_gattc_primary_services_discover(conn_handle,
                                                              p_req->params.gattc_srv_disc.start_handle,
                                                              &p_req->params.gattc_srv_disc.srvc_uuid);
            break;

        case NRF_BLE_GQ_REQ_CHAR_DISCOVERY:
            NRF_LOG_DEBUG("GATTC Characteristic Discovery Request");
            err_code = sd_ble_gattc_characteristics_discover(conn_handle,
                                                             &p_req->params.gattc_char_disc);
            break;

        case NRF_BLE_GQ_REQ_DESC_DISCOVERY:
            NRF_LOG_DEBUG("GATTC Characteristic Descriptor Request");
            err_code = sd_ble_gattc_descriptors_discover(conn_handle,
                                                         &p_req->params.gattc_desc_disc);
            break;

        case NRF_BLE_GQ_REQ_GATTS_HVX:
        {
            uint16_t len = *p_req->params.gatts_hvx.p_len;

            NRF_LOG_DEBUG("GATTS Notification or Indication");

            err_code = sd_ble_gatts_hvx(conn_handle,
                                        &p_req->params.gatts_hvx);

            if ((err_code == NRF_SUCCESS) &&
                (len != *p_req->params.gatts_hvx.p_len))
            {
                err_code = NRF_ERROR_DATA_SIZE;
            }

        } break;

        default:
            NRF_LOG_WARNING("Unimplemented GATT Request");
            break;
    }

    if (err_code == NRF_ERROR_BUSY) // Softdevice is processing another GATT request.
    {
        NRF_LOG_DEBUG("SD is currently busy. The GATT request procedure will be attempted \
                      again later.");
        return false;
    }
    else
    {
        request_err_code_handle(p_req, conn_handle, err_code);
        return true;
    }
}


/**@brief Function finds ID for the provided connection handle within nrf_ble_gq_t instance registry.
 *
 * @param[in] p_gatt_queue  Pointer to the nrf_ble_gq_t instance.
 * @param[in] conn_handle   Connection handle.
 *
 * @return    Connection ID.
 */
static uint16_t conn_handle_id_find(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
{
    uint16_t id;

    for (id = 0; id < p_gatt_queue->max_conns; id++)
    {
        if (conn_handle == p_gatt_queue->p_conn_handles[id])
        {
            return id;
        }
    }
    return id;
}


/**@brief Function registers provided connection handle within nrf_ble_gq_t instance registry.
 *
 * @param[in] p_gatt_queue  Pointer to the nrf_ble_gq_t instance.
 * @param[in] conn_handle   Connection handle.
 *
 * @retval    NRF_SUCCESS       If the registration was successful.
 * @retval    NRF_ERROR_NO_MEM  If there was no space for another connection handle.
 */
static ret_code_t conn_handle_register(nrf_ble_gq_t const * const p_gatt_queue, uint16_t conn_handle)
{
    for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
    {
        if (p_gatt_queue->p_conn_handles[id] == BLE_CONN_HANDLE_INVALID)
        {
            p_gatt_queue->p_conn_handles[id] = conn_handle;
            return NRF_SUCCESS;
        }
    }
    return NRF_ERROR_NO_MEM;
}


/**@brief Function checks if any connection handle is registered in nrf_ble_gq_t instance.
 *
 * @param[in] p_gatt_queue Pointer to the nrf_ble_gq_t instance.
 *
 * @retval    true   There is at least one registered connection handle.
 * @retval    false  Connection handle registry is empty.
 */
static bool is_any_conn_handle_registered(nrf_ble_gq_t const * const p_gatt_queue)
{
    for (uint16_t id = 0; id < p_gatt_queue->max_conns; id++)
    {
        if (p_gatt_queue->p_conn_handles[id] != BLE_CONN_HANDLE_INVALID)
        {
            return true;
        }
    }
    return false;
}


ret_code_t nrf_ble_gq_item_add(nrf_ble_gq_t const * const p_gatt_queue,
                               nrf_ble_gq_req_t   * const p_req,
                               uint16_t                   conn_handle)
{
    ret_code_t err_code = NRF_SUCCESS;
    uint16_t   conn_id;

    NRF_LOG_DEBUG("Adding item to the request queue");

    VERIFY_PARAM_NOT_NULL(p_gatt_queue);
    VERIFY_PARAM_NOT_NULL(p_req);

    // Purge queues that are no longer used by any connection.
    queues_purge(p_gatt_queue);

    // Check if connection handle is registered and if GATT request is valid.
    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
    if ((p_req->type >= NRF_BLE_GQ_REQ_NUM) || (conn_id == p_gatt_queue->max_conns))
    {
        return NRF_ERROR_INVALID_PARAM;
    }

    // Try processing a request without buffering.
    if (nrf_queue_is_empty(&p_gatt_queue->p_req_queue[conn_id]))
    {
        bool req_processed = request_process(p_req, conn_handle);
        if (req_processed)
        {
            return err_code;
        }
    }

    // Prepare request for buffering and add it to the queue.
    if (m_req_data_alloc[p_req->type] != NULL)
    {
        VERIFY_PARAM_NOT_NULL(p_gatt_queue->p_data_pool);

        err_code = m_req_data_alloc[p_req->type](p_gatt_queue->p_data_pool, p_req);
        VERIFY_SUCCESS(err_code);
    }

    err_code = nrf_queue_push(&p_gatt_queue->p_req_queue[conn_id], p_req);
    if ((err_code != NRF_SUCCESS) && (m_req_data_alloc[p_req->type] != NULL))
    {
        nrf_memobj_free(p_req->p_mem_obj);
        NRF_LOG_DEBUG("Pointer to freed memory block: %p.", p_req->p_mem_obj);
    }

    // Check if Softdevice is still busy.
    queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
    return err_code;
}


ret_code_t nrf_ble_gq_conn_handle_register(nrf_ble_gq_t * const p_gatt_queue, uint16_t conn_handle)
{
    ret_code_t err_code = NRF_SUCCESS;
    uint16_t   conn_id;

    VERIFY_PARAM_NOT_NULL(p_gatt_queue);

    // Purge queues that are no longer used by any connection.
    queues_purge(p_gatt_queue);

    // Allow instance to claim connection handle only if it has not been claimed already.
    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
    if (conn_id == p_gatt_queue->max_conns)
    {
        NRF_LOG_DEBUG("Registering connection handle: 0x%04X", conn_handle);

        // Initialize/reset data pool if possible.
        if (!is_any_conn_handle_registered(p_gatt_queue))
        {
            err_code = nrf_memobj_pool_init(p_gatt_queue->p_data_pool);
        }

        err_code = conn_handle_register(p_gatt_queue, conn_handle);
        VERIFY_SUCCESS(err_code);
    }
    return err_code;
}


void nrf_ble_gq_on_ble_evt(ble_evt_t const * p_ble_evt, void * p_context)
{
    nrf_ble_gq_t * p_gatt_queue = (nrf_ble_gq_t *) p_context;
    uint16_t       conn_handle;
    uint16_t       conn_id;

    if ((p_ble_evt == NULL) || (p_gatt_queue == NULL))
    {
        return;
    }

    // Obtain connection handle and filter out the events that do not trigger queue processing.
    if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
    {
        conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
    }
    else if ((p_ble_evt->header.evt_id >= BLE_GATTC_EVT_BASE) &&
             (p_ble_evt->header.evt_id <= BLE_GATTC_EVT_LAST))
    {
        conn_handle = p_ble_evt->evt.gattc_evt.conn_handle;
    }
    else if ((p_ble_evt->header.evt_id >= BLE_GATTS_EVT_BASE) &&
             (p_ble_evt->header.evt_id <= BLE_GATTS_EVT_LAST))
    {
        conn_handle = p_ble_evt->evt.gatts_evt.conn_handle;
    }
    else
    {
        // These events are irrelevant for this module.
        return;
    }

    // Check if connection handle is registered.
    conn_id = conn_handle_id_find(p_gatt_queue, conn_handle);
    if (conn_id == p_gatt_queue->max_conns)
    {
        return;
    }

    // Perform operations on the queue.
    if (p_ble_evt->header.evt_id == BLE_GAP_EVT_DISCONNECTED)
    {
        p_gatt_queue->p_conn_handles[conn_id] = BLE_CONN_HANDLE_INVALID;
        UNUSED_RETURN_VALUE(nrf_queue_push(p_gatt_queue->p_purge_queue, &conn_id));
    }
    else
    {
        queue_process(&p_gatt_queue->p_req_queue[conn_id], conn_handle);
    }
}


#endif // NRF_MODULE_ENABLED(NRF_BLE_GQ)
