/*
 * ringbuffer.h
 *
 *  Created on: Sep 14, 2021
 *      Author: Attila Body
 */

#pragma once

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <FreeRTOS.h>
#include <semphr.h>
#include <task.h>

namespace fsl {

template<size_t capacity> class RingBuffer
{
public:
    /**
     * @brief Initializes the ring buffer internal structure with the corresponding parameters.
     *        Provided only for convenience, the structure also can be initialized locally
     */
    RingBuffer(TaskHandle_t consumerTask,
//            BaseType_t consumerNotifyIndex,
            uint32_t consumerNotifyMask,
            TaskHandle_t producerTask,
//            BaseType_t producerNotifyIndex,
            uint32_t producerNotifyMask);

    /**
     * @brief Copies data to the ring buffer (without committing it) The amount of the committed data in the buffer
     *        should not exceed the size of the buffer.
     * @param data Pointer to the data to copy
     * @param len Length of the data to copy
     */
    void Put(uint8_t const *data_buffer, size_t len);


    /**
     * @brief Commits the data already placed into the buffer and notifies the consumer about it's availability
     */
    void Commit();


    /**
     * @brief Waits until all the data from the ring buffer gets consumed.
     */
    void Flush();


    /**
     * @brief Gets a pointer to the next chunk of committed data in the buffer without administering the consumption
     *        of it. The caller should also call ringbuffer_RepostConsumption using the returned chunk length after
     *        it finished processing the data.
     * @param[in]  len_requested Length of the data requested from the buffer. The length of the actual data provided
     *             might be actually smaller (because either reaching the end of the buffer or not enough data in the buffer).
     * @param[out] data Receives 1a pointer to the first byte of the available data in the buffer
     * @param[out] len Receives the length of the chunk available in the buffer. Will not exceed len_requested.
     * @retval true if the buffer has more available data, false otherwise
     * @remark The caller should wait for notification from the producer task (using m_producerNotifyIndex
     *         and producer_notify_mask) before calling this function and consume the data
     *         (also registering each consumption cycle) until this function returns false
     */
    bool GetChunk(size_t len_requested, uint8_t *&data, size_t &len);


    /**
     * @brief Marks the chunk returned by ringbuffer_GetChunk as available
     * @param consumed The length of the chunk as returned by ringbuffer_GetChunk(..., len)
     */
    void ReportConsumption(size_t consumed);


private:
    size_t          m_head;                 //!< Write position
    size_t          m_headShadow;           //!< Shadowed write position for collecting data before committing it
    size_t          m_tail;                 //!< Read position
    TaskHandle_t    m_consumerTask;         //!< Task handle of the consumer (to notify when data is available)
    BaseType_t      m_consumerNotifyIndex;  //!< Notification index used for consumer notification
    uint32_t        m_consumerNotifyMask;   //!< Notification data for the consumer
    TaskHandle_t    m_producerTask;         //!< Task handle of the producer (to notify when available buffer space increased)
    BaseType_t      m_producerNotifyIndex;  //!< Notification index used for producer notification
    uint32_t        m_producerNotifyMask;   //!< Notification data for the producer

    uint8_t         m_buffer[capacity];         //!< Pointer to the phisical memory bufer

    static inline size_t used(size_t s, size_t h, size_t t)
    {
        return (((h) >= (t)) ? ((h) - (t)) : ((s) - (t) + (h)));
    }

    static inline size_t available(size_t s, size_t h, size_t t)
    {
        return ((s) - used(s, h, t));
    }

    void __ASSERT(bool pred)
    {
    }

};

template<size_t capacity> RingBuffer<capacity>::RingBuffer(
        TaskHandle_t consumerTask,
//        BaseType_t consumerNotifyIndex,
        uint32_t consumerNotifyMask,
        TaskHandle_t producerTask,
//        BaseType_t producerNotifyIndex,
        uint32_t producerNotifyMask)
        : m_consumerTask(consumerTask)
//        , m_consumerNotifyIndex(consumerNotifyIndex)
        , m_consumerNotifyMask(consumerNotifyMask)
        , m_producerTask(producerTask)
//        , m_producerNotifyIndex(producerNotifyIndex)
        , m_producerNotifyMask(producerNotifyMask)
{}


template<size_t capacity> void RingBuffer<capacity>::Put(uint8_t const *data_buffer, size_t len)
{
    uint16_t chunk1 = 0;
    uint16_t chunk2 = 0;
    uint16_t uncommitted = 0;
    uncommitted = used(capacity, m_headShadow, m_head);

    __ASSERT(uncommitted + len + 1 <= capacity);

    if(xPortIsInsideInterrupt()) {
        if(available(capacity, m_headShadow, m_tail) < len + 1) {
            __ASSERT(false);
            return;
        }
    } else {
        while(available(capacity, m_headShadow, m_tail) < len + 1) {
//            xTaskNotifyWaitIndexed(m_producerNotifyIndex, 0, m_producerNotifyMask, NULL, portMAX_DELAY);
            xTaskNotifyWait(0, m_producerNotifyMask, NULL, portMAX_DELAY);
        }
    }

    chunk1 = capacity - m_headShadow;
    if(chunk1 >= len) {
        chunk1 = len;
    } else {
        chunk2 = len - chunk1;
    }

    memcpy(m_buffer + m_headShadow, data_buffer, chunk1);
    m_headShadow += chunk1;
    if(m_headShadow == capacity) {
        m_headShadow = 0;
    }

    if(chunk2) {
        memcpy(m_buffer, data_buffer + chunk1, chunk2);
        m_headShadow += chunk2;
        if(m_headShadow == capacity) {
            m_headShadow = 0;
        }
    }
}


template<size_t capacity> void RingBuffer<capacity>::Commit()
{
    uint16_t uncommitted = used(capacity, m_headShadow, m_head);

    if(!uncommitted) {
        return;
    }

    m_head = m_headShadow;

    if(xPortIsInsideInterrupt()) {
        BaseType_t woken_up = 0;
//        xTaskNotifyIndexedFromISR(m_consumerTask, m_consumerNotifyIndex, m_consumerNotifyMask, eSetBits, &woken_up);
        xTaskNotifyFromISR(m_consumerTask, m_consumerNotifyMask, eSetBits, &woken_up);
        portYIELD_FROM_ISR(woken_up); // NOLINT(hicpp-no-assembler)
    } else {
//        xTaskNotifyIndexed(m_consumerTask, m_consumerNotifyIndex, m_consumerNotifyMask, eSetBits);
        xTaskNotify(m_consumerTask, m_consumerNotifyMask, eSetBits);
    }

}


template<size_t capacity> void RingBuffer<capacity>::Flush()
{
     while(m_head != m_tail) {
        // TODO: Watchdog?
//         xTaskNotifyWaitIndexed(m_producerNotifyIndex, 0, m_producerNotifyMask, NULL, portMAX_DELAY);
         xTaskNotifyWait(0, m_producerNotifyMask, NULL, portMAX_DELAY);
    }
}


template<size_t capacity> bool RingBuffer<capacity>::GetChunk(size_t len_requested, uint8_t *&data, size_t &len)
{
    size_t head = m_head;
    size_t tail = m_tail;
    size_t chunk_size = head >= tail ? head - tail : capacity - tail;
    if(!chunk_size) {
        len = 0;
        return false;
    }
    if(chunk_size > len_requested) {
        chunk_size = len_requested;
    }
    data = m_buffer + tail;
    len = chunk_size;

    tail += chunk_size;
    if(tail == capacity) {
        tail = 0;
    }
    return tail != head;
}


template<size_t capacity> void RingBuffer<capacity>::ReportConsumption(size_t consumed)
{
    if(!consumed) {
        return;
    }
    m_tail += consumed;
    if(m_tail == capacity) {
        m_tail = 0;
    }
    if(m_producerTask) {
        if(xPortIsInsideInterrupt()) {
            BaseType_t woken_up = 0;
//            xTaskNotifyIndexedFromISR(m_producerTask, m_producerNotifyIndex, m_producerNotifyMask, eSetBits, &woken_up);
            xTaskNotifyFromISR(m_producerTask, m_producerNotifyMask, eSetBits, &woken_up);
            portYIELD_FROM_ISR(woken_up); // NOLINT(hicpp-no-assembler)
        } else {
//            xTaskNotifyIndexed(m_producerTask, m_producerNotifyIndex, producerNotifyMask, eSetBits);
            xTaskNotify(m_producerTask, m_producerNotifyMask, eSetBits);
        }
    }
}

}   // fsl