// queue.h -- interface for a queue
#ifndef __LINKED_LOCK_FREE_QUEUE_H_
#define __LINKED_LOCK_FREE_QUEUE_H_
#include "mm.h" 
#include "sem_util.h"

template <typename T> class Node;

template <typename T>
class Pointer {
public:

    Node<T> *ptr;
    unsigned long count;
    Pointer( Node<T> *node = NULL, int c=0) noexcept :  ptr(node), count(c) {}
     
    bool operator == (const Pointer<T> o) const {
      return (o.ptr == ptr) && (o.count == count);
    }
    bool operator != (const Pointer<T> o) const {
      return !((o.ptr == ptr) && (o.count == count));
    }

    
   
};

template <typename T>
class Node {
public:
    alignas(16) std::atomic<Pointer<T> > next;
    T value; 
    
    Node() {
    }

    void *operator new(size_t size){
        return mm_malloc(size);
    }

    void operator delete(void *p) {
        return mm_free(p);
    }
};





template <typename ELEM_T>
class LinkedLockFreeQueue
{

    template <
        typename ELEM_T_, 
        template <typename T> class Q_TYPE >
    friend class LockFreeQueue;
private:
// class scope definitions
    enum {Q_SIZE = 10};
  
// private class members
    std::atomic<Pointer<ELEM_T> > Head;       // pointer to front of Queue
    std::atomic<Pointer<ELEM_T> > Tail;        // pointer to rear of Queue
    //std::atomic_uint count;          // current number of size in Queue
    std::atomic_uint count;
    const size_t qsize;    // maximum number of size in Queue
    // preemptive definitions to prevent public copying
    LinkedLockFreeQueue(const LinkedLockFreeQueue & q) : qsize(0) { }
    LinkedLockFreeQueue & operator=(const LinkedLockFreeQueue & q) { return *this;}
protected:
    LinkedLockFreeQueue(size_t qs = Q_SIZE); // create queue with a qs limit
    ~LinkedLockFreeQueue();
    bool empty() const;
    bool full() const;
    unsigned int size() const;
    bool push(const ELEM_T &item); // add item to end
    bool pop(ELEM_T &item);
    

    ELEM_T& operator[](unsigned i);

};


// Queue methods
template <typename T>
LinkedLockFreeQueue<T>::LinkedLockFreeQueue(size_t qs) : count(0), qsize(qs)
{
    Node<T> *node = new Node<T>;
    Pointer<T> pointer(node, 0);
    
    Head.store(pointer, std::memory_order_relaxed);
    Tail.store(pointer, std::memory_order_relaxed);

}

template <typename T>
LinkedLockFreeQueue<T>::~LinkedLockFreeQueue()
{
    LoggerFactory::getLogger()->debug("LinkedLockFreeQueue destory");
    Node<T> * nodeptr;
    Pointer<T> tmp = Head.load(std::memory_order_relaxed);
    while((nodeptr = tmp.ptr) != NULL) {
        tmp = (tmp.ptr->next).load(std::memory_order_relaxed);
        //std::cerr << "delete " << nodeptr << std::endl;
        delete nodeptr;

    }
}

template <typename T>
bool LinkedLockFreeQueue<T>::empty() const
{
    return count == 0;
}

template <typename T>
bool LinkedLockFreeQueue<T>::full() const
{
    return count == qsize;
}

template <typename T>
unsigned int LinkedLockFreeQueue<T>::size() const
{
    return count;
}

// Add item to queue
template <typename T>
bool LinkedLockFreeQueue<T>::push(const T & item)
{
    if (full())
        return false;

    Node<T> * node = new Node<T>;
    node->value = item;
   

    Pointer<T> tail ;
    Pointer<T> next ;
   

    while(true) {
        tail = Tail.load(std::memory_order_relaxed);
        next = (tail.ptr->next).load(std::memory_order_relaxed);
        if (tail == Tail.load(std::memory_order_relaxed)) {
            if (next.ptr == NULL) {
                if ((tail.ptr->next).compare_exchange_weak(next, 
                    Pointer<T>(node, next.count+1), 
                    std::memory_order_release, 
                    std::memory_order_relaxed) )
                    break;
                else
                    Tail.compare_exchange_weak(tail, 
                        Pointer<T>(next.ptr, tail.count+1),
                        std::memory_order_release, 
                        std::memory_order_relaxed);
            }

        }
    }

    Tail.compare_exchange_weak(tail, Pointer<T>(node, tail.count+1), 
        std::memory_order_release, 
        std::memory_order_relaxed);  
    count++;
    return true;
}




// Place front item into item variable and remove from queue
template <typename T>
bool LinkedLockFreeQueue<T>::pop(T & item)
{
    if (empty())
        return false;
        
    Pointer<T> head;
    Pointer<T> tail;
    Pointer<T> next;

    while(true) {
        head = Head.load(std::memory_order_relaxed);
        tail = Tail.load(std::memory_order_relaxed);
        next = (head.ptr->next).load();
        if (head == Head.load(std::memory_order_relaxed)) {
            if(head.ptr == tail.ptr) {
                if (next.ptr == NULL)
                    return false;
                // Tail is falling behind. Try to advance it
                Tail.compare_exchange_weak(tail, 
                        Pointer<T>(next.ptr, tail.count+1),
                        std::memory_order_release, 
                        std::memory_order_relaxed);
            } else {
                item = next.ptr->value;
                if (Head.compare_exchange_weak(head, 
                        Pointer<T>(next.ptr, head.count+1), 
                        std::memory_order_release, 
                        std::memory_order_relaxed)) {
                  delete head.ptr;
                  break;  
                }

            }
        }        

    }

    count--; 
    return true;
            
}

 
template <class T>
T& LinkedLockFreeQueue<T>::operator[](unsigned int i)
{
    if (i < 0 || i >= count)
    {
        std::cerr << "Error in array limits: " << i << " is out of range\n";
        std::exit(EXIT_FAILURE);
    }


    Pointer<T> tmp = Head.load(std::memory_order_relaxed);
    //Pointer<T> tail = Tail.load(std::memory_order_relaxed);

    while(i > 0) {
        //std::cout << i << ":"  << std::endl;
        tmp = (tmp.ptr->next).load(std::memory_order_relaxed);
        i--;    
    }

    tmp = (tmp.ptr->next).load(std::memory_order_relaxed);
    return tmp.ptr->value;
}



#endif