queue.hpp

#pragma once
 
#include "../util/util.hpp"
 
#include <iostream>
#include <vector>
#include <iterator>
#include <algorithm>
#include <concepts>
#include <thread>
 
namespace gz {
 
    // from util.hpp
    template<std::unsigned_integral I, std::unsigned_integral S>
    inline void incrementIndex(I& i, const S containerSize) {
        if (i < containerSize - 1) { i++; }
        else { i = 0; }
    }
    template<std::unsigned_integral I, std::unsigned_integral S>
    inline void decrementIndex(I& i, const S containerSize) {
        if (i > 0) { i--; }
        else { i = containerSize - 1; }
    }
    template<std::unsigned_integral I, std::unsigned_integral S>
    inline I getIncrementedIndex(const I i, const S containerSize) {
        if (i < containerSize - 1) { return i + 1; }
        else { return 0; }
    }
    template<std::unsigned_integral I, std::unsigned_integral S>
    inline I getDecrementedIndex(const I i, const S containerSize) {
        if (i > 0) { return i - 1; }
        else { return containerSize - 1; }
    }
    template<std::integral I, std::unsigned_integral S>
    size_t getValidIndex(const I i, const S containerSize) {
        if (i < 0) { 
            return containerSize - (-i) % containerSize - 1;
        }
        else if (i >= static_cast<int>(containerSize)) {
            return i % containerSize;
        }
        return i;
    }
    template<std::swappable T>
    class Queue {
        public:
            Queue(size_t size=10, size_t maxSize=-1);
 
            void push_back(T& t);
            void push_back(T&& t) { push_back(t); };
            void emplace_back(T&& t);
 
            bool hasElement();
            T& getRef();
            T getCopy();
 
            void clear();
 
            std::vector<T>& getInternalBuffer() { return buffer; }
        private:
            void resize();
 
            size_t writeIndex;  
            size_t readIndex;  
            std::vector<T> buffer;
            size_t vectorCapacity;
            size_t maxSize;
            std::mutex mtx;
    };
 
    template<std::swappable T>
    Queue<T>::Queue(size_t capacity, size_t maxSize)
        : vectorCapacity(capacity), maxSize(maxSize) {
        buffer.reserve(capacity);
        /* buffer.resize(2); */
 
        writeIndex = capacity - 1;
        readIndex = capacity - 1;
    }
 
 
    template<std::swappable T>
    void Queue<T>::resize() {
        // if vector is at maxSize, "loose" the oldest element
        if (buffer.size() == maxSize) {
            incrementIndex(readIndex, buffer.size());
            return;
        }
        // if not at end of vector rotate so that oldest element is first
        if (writeIndex != vectorCapacity - 1) {
            // if not at end, reserve more space and move elements
            std::rotate(buffer.begin(), buffer.begin() + readIndex, buffer.end());
            readIndex = 0;
            writeIndex = vectorCapacity - 1;
        } 
        // reserve 10% more space (at least space for 3 more elements).
        buffer.reserve(std::min(std::max(static_cast<size_t>(1.1 * vectorCapacity), vectorCapacity + 3), maxSize));
        vectorCapacity = buffer.capacity();
    }
 
 
    template<std::swappable T>
    void Queue<T>::push_back(T& t) {
        mtx.lock();
        // check if this would write into oldest element
        if (readIndex == getIncrementedIndex(writeIndex, vectorCapacity)) { resize(); }
 
        util::incrementIndex(writeIndex, vectorCapacity);
        if (buffer.size() < vectorCapacity) {
            buffer.push_back(t); 
        }
        else {
            buffer[writeIndex] = t;
        }
        mtx.unlock();
        /* std::cout << "queue after pushback. ri: " << readIndex << " - wi: " << writeIndex << " - size: " << buffer.size() << " - cap: " << vectorCapacity << "\n"; */
    }
 
 
    template<std::swappable T>
    void Queue<T>::emplace_back(T&& t) {
        mtx.lock();
        if (readIndex == getIncrementedIndex(writeIndex, vectorCapacity)) { resize(); }
 
        util::incrementIndex(writeIndex, vectorCapacity);
        if (buffer.size() < vectorCapacity) {
            buffer.emplace_back(std::move(t)); 
        }
        else {
            buffer[writeIndex] = std::move(t);
        }
        mtx.unlock();
    }
    
 
    template<std::swappable T>
    bool Queue<T>::hasElement() {
        mtx.lock();
        bool hasElement = writeIndex != readIndex;
        mtx.unlock();
        return hasElement;
    }
 
    
    template<std::swappable T>
    T& Queue<T>::getRef() {
        mtx.lock();
        incrementIndex(readIndex, vectorCapacity);
        size_t i = readIndex;
        T& element = buffer[i];
        mtx.unlock();
        return buffer[i];
    }
 
 
    template<std::swappable T>
    T Queue<T>::getCopy() {
        mtx.lock();     
        incrementIndex(readIndex, vectorCapacity);
        size_t i = readIndex;
        mtx.unlock();
        return std::move(buffer[i]);  
    }
 
 
    template<std::swappable T>
    void Queue<T>::clear() {
        mtx.lock();     
        writeIndex = 0;
        readIndex = 0;
        mtx.unlock();
    }
} // namespace gz