#include <Os/LocklessQueue.hpp>
#include <Fw/Types/Assert.hpp>

#include <fcntl.h>
#include <cstring>
#include <new>

#define CAS(a_ptr, a_oldVal, a_newVal) __sync_bool_compare_and_swap(a_ptr, a_oldVal, a_newVal)

namespace Os {

    LocklessQueue::LocklessQueue(const NATIVE_INT_TYPE maxmsg,
            const NATIVE_INT_TYPE msgsize) {
#ifndef BUILD_DARWIN
        m_attr.mq_flags = O_NONBLOCK;
        m_attr.mq_maxmsg = maxmsg;
        m_attr.mq_msgsize = msgsize;
        m_attr.mq_curmsgs = 0;
#endif
        // Must have at least 2 messages in the queue
        FW_ASSERT(maxmsg >= 2, maxmsg);

        m_index = new(std::nothrow) QueueNode[maxmsg]; // Allocate an index entry for each msg
        m_data = new(std::nothrow) U8[maxmsg * msgsize];  // Allocate data for each msg
        FW_ASSERT(m_index != nullptr);
        FW_ASSERT(m_data != nullptr);

        for (int i = 0, j = 1; i < maxmsg; i++, j++) {
            m_index[i].data = &m_data[i * msgsize]; // Assign the data pointer of index to top that msg's buffer

            if (j < maxmsg) {           // If we aren't processing the last item
                m_index[i].next = &m_index[j]; // Chain this item to the next one
            } else {
                m_index[i].next = nullptr;         // Initialize to NULL otherwise
            }
        }

        // Assign each of the pointers to the first index element
        // This one is held in a "dummy" position for now
        // It will be cleaned up by the producer
        m_first = &m_index[0];
        m_last = &m_index[0];
        m_last->next = nullptr;

        // Assign the head of the free list to the second element
        m_free_head = &m_index[1];
    }

    LocklessQueue::~LocklessQueue() {
        delete[] m_index;
        delete[] m_data;
    }

#ifndef BUILD_DARWIN
    void LocklessQueue::GetAttr(mq_attr & attr) {
        memcpy(&attr, &m_attr, sizeof(mq_attr));
    }
#endif

    void LocklessQueue::PushFree(QueueNode * my_node) {
        QueueNode * old_free_head;

        FW_ASSERT(my_node != nullptr);

        // CAS the node into the free list
        do {
            old_free_head = m_free_head;
            my_node->next = old_free_head;
        } while (!CAS(&m_free_head, old_free_head, my_node));
    }

    bool LocklessQueue::PopFree(QueueNode ** free_node) {
        QueueNode * my_node;

        // CAS a buffer from the free list
        do {
            my_node = m_free_head;

            if (nullptr == my_node) {
                return false;
            }

        } while (!CAS(&m_free_head, my_node, my_node->next));

        (*free_node) = my_node;

        return true;
    }

    Queue::QueueStatus LocklessQueue::Send(const U8 * buffer,
            NATIVE_INT_TYPE size) {

        QueueNode * my_node;
        QueueNode * old_last;

#ifndef BUILD_DARWIN
        // Check that the new message will fit in our buffers
        if (size > m_attr.mq_msgsize) {
            return Queue::QUEUE_SIZE_MISMATCH;
        }
#endif

        if (!PopFree(&my_node)) {
            return Queue::QUEUE_FULL;
        }

        // Copy the data into the buffer
        memcpy(my_node->data, buffer, size);
        my_node->size = size;
        my_node->next = nullptr;

        // Publish the node
        // The m_last pointer is moved before the item is published
        // All concurrent writers will argue over their order using CAS
        do {
            old_last = m_last;
        } while (!CAS(&m_last, old_last, my_node));

        old_last->next = my_node;

        return Queue::QUEUE_OK;
    }

    // ONLY ONE RECEIVER AT A TIME!!!!
    Queue::QueueStatus LocklessQueue::Receive(U8 * buffer,
            NATIVE_INT_TYPE capacity, NATIVE_INT_TYPE & size) {

        QueueNode * my_node;
        QueueNode * old_node;

#ifndef BUILD_DARWIN
        if (capacity < m_attr.mq_msgsize) {
            return Queue::QUEUE_SIZE_MISMATCH;
        }
#endif

        if (m_first == m_last) {
            return Queue::QUEUE_NO_MORE_MSGS;
        }

        old_node = m_first;
        my_node = m_first->next;

        if (my_node == nullptr) {
            // We may not be fully linked yet, even though "last" has moved
            return Queue::QUEUE_NO_MORE_MSGS;
        }

        m_first = m_first->next;
        PushFree(old_node);

        // Copy the data from the buffer
        memcpy(buffer, my_node->data, my_node->size);
        size = my_node->size;

        return Queue::QUEUE_OK;
    }

}
