/*
 * eth_ecat_queue.hpp
 *
 *  Created on: Jun 2, 2023
 *      Author: algin
 */

#ifndef FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_QUEUE_HPP_
#define FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_QUEUE_HPP_

#include <atomic>

namespace free_rtos {

namespace queue {

class QueueEntity {
public:

protected:
    QueueEntity* get_next() {
        return next_;
    }

    void set_next(QueueEntity *next) {
        next_ = next;
    }

private:
    QueueEntity *next_{nullptr};
};


template<typename DataType>
class Queue {
    static_assert(std::is_base_of<QueueEntity, DataType>::value == true, "DataType should be derived from QueueEntity and Queue should be a friend class");

public:
    Queue() { }

    explicit Queue(DataType& first) {
        //DebugP_log((char*)"Constructor coupling first data\r\n");

        queue(&first);
    }

    Queue(DataType& first, DataType& next) {
        //DebugP_log((char*)"Constructor coupling first and next data\r\n");

        first.set_next(&next);

        queue(&first, &next, 2);
    }

    DataType* get_first() {
        return first_;
    }

    DataType* get_last() {
        return last_;
    }

    size_t get_size() {
        return size_;
    }

    Queue& operator+(DataType& data) {
        //DebugP_log((char*)"Coupling next data\r\n");

        return queue(&data);
    }

    Queue& operator+(Queue& other) {
        //DebugP_log((char*)"Coupling with other queue\r\n");

        return queue(other.get_first(), other.get_last(), other.get_size());
    }

    DataType* dequeue() {
        DataType* first = first_;

        if(first == nullptr) {
            return nullptr;
        }

        first_ = static_cast<DataType*>(first_->get_next());

        if(first_ == nullptr) {
            last_ = nullptr;
        }

        size_--;

        return first;
    }

    bool empty() {
        return (first_ == nullptr);
    }

    void clear() {
        first_ = nullptr;
        last_ = nullptr;
        size_ = 0;
    }

private:
    DataType *first_{nullptr};
    DataType *last_{nullptr};
    size_t size_{0};

    Queue& queue(DataType *other_first, DataType *other_last = nullptr, size_t other_size = 1) {
        if(first_ == nullptr) {
            first_ = other_first;
        }

        if(last_ != nullptr) {
            last_->set_next(other_first);
        }

        if(other_last == nullptr) {
            last_ = other_first;
        }else{
            last_ = other_last;
        }

        size_ += other_size;

        //DebugP_log((char*)"size_ = %d\r\n", size_);

        return *this;
    }
};

}

}

#endif /* FREE_RTOS_ETHERNET_INDUSTRY_ETH_ECAT_QUEUE_HPP_ */