#pragma once

#include <queue>
#include <mutex>
#include <condition_variable>

namespace ROSBridge {

template <class T>
class ThreadSafeQueue
{
public:
    ThreadSafeQueue();
    ~ThreadSafeQueue();

    T pop_front();

    void push_back(const T& item);
    void push_back(T&& item);

    int size();
    bool empty();

private:
    std::deque<T> _queue;
    std::mutex _mutex;
    std::condition_variable _cond;
};

template <typename T>
ThreadSafeQueue<T>::ThreadSafeQueue(){}

template <typename T>
ThreadSafeQueue<T>::~ThreadSafeQueue(){}

template <typename T>
T ThreadSafeQueue<T>::pop_front()
{
    std::unique_lock<std::mutex> mlock(_mutex);
    while (_queue.empty())
    {
        _cond.wait(mlock);
    }
    T t = std::move(_queue.front());
    _queue.pop_front();
    return t;
}

template <typename T>
void ThreadSafeQueue<T>::push_back(const T& item)
{
    std::unique_lock<std::mutex> mlock(_mutex);
    _queue.push_back(item);
    mlock.unlock();     // unlock before notificiation to minimize mutex con
    _cond.notify_one(); // notify one waiting thread

}

template <typename T>
void ThreadSafeQueue<T>::push_back(T&& item)
{
    std::unique_lock<std::mutex> mlock(_mutex);
    _queue.push_back(std::move(item));
    mlock.unlock();     // unlock before notificiation to minimize mutex con
    _cond.notify_one(); // notify one waiting thread

}

template <typename T>
int ThreadSafeQueue<T>::size()
{
    std::unique_lock<std::mutex> mlock(_mutex);
    int size = _queue.size();
    mlock.unlock();
    return size;
}

} // namespace