/*
   HARK Library
   Copyright (C) 2021 Honda Research Institute Japan Inc.
 */

#include "hark-lib/datastream.h"
#include "hark-lib/error.h"
#include "hark-lib/dispatch.h"

#include "hark-lib/internal/mutex.h"
#include "hark-lib/internal/condition_variable.h"
#include "hark-lib/internal/lock.h"

#include <condition_variable>

namespace hark
{
    const static size_t queue_margin = 1;

    QueueingDataProxy::QueueingDataProxy(BaseNode *parent, const std::string &name):
        DataProxy(parent, name), _look_back(0), _look_ahead(0), _look_back_required(0), _look_ahead_required(0), _extended_window(0), _queue_margin(0), _is_static(false), _is_synchronous(true), _count(0), _head_cursor(0), _upstream_delay(0), _mutex(new Mutex()), _condvar(new ConditionVariable())
    {
    }

    QueueingDataProxy::~QueueingDataProxy()
    {
    }

    void
    QueueingDataProxy::clear()
    {
        this->_count = 0;
        this->_head_cursor = 0;
        this->_queue.clear();
    }

    size_t
    QueueingDataProxy::count() const
    {
        return this->_count;
    }

    data
    QueueingDataProxy::last()
    {
        return this->relative(0);
    }

    data
    QueueingDataProxy::relative(int offset)
    {
        if(this->_is_static){
            return data(this->_static_value);

        }else{
            if(offset < 0 && (size_t)(-offset) > this->_count){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data beyond look-back window";
                throw RuntimeError(ss.str());
            }
            return this->at(offset + this->_count);
        }
    }

    data
    QueueingDataProxy::at(size_t count)
    {
        if(this->_is_static){
            return data(this->_static_value);

        }else{
            UniqueLock lock(this->_mutex.get());
            //std::unique_lock<std::mutex>lock(this->_mutex);

            if(count + this->_look_back < this->_count){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data beyond look-back window";
                throw RuntimeError(ss.str());
            }
            if(count > 1 + this->_look_ahead + this->_look_back + this->_count){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data beyond look-ahead window";
                throw RuntimeError(ss.str());
            }

            size_t actual_offset = count - this->_head_cursor;
            if(actual_offset >= this->_queue.size()){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data not ready: count=" << count << ", _count=" << this->_count << ". _head_cursor=" << this->_head_cursor << ", queue-size=" << this->_queue.size();
                throw RuntimeError(ss.str());
            }

            // std::cout << "QueueingDataProxy: at: " << actual_offset << ": " << count << ", " << this->_count << ", " << this->_look_back << std::endl;
            return this->_queue.at(actual_offset);
        }
    }

    bool
    QueueingDataProxy::is_available_at(size_t count) const
    {
        if(this->_is_static){
            return true;

        }else{
            UniqueLock lock(this->_mutex.get());
            //std::unique_lock<std::mutex>lock(this->_mutex);

            if(count + this->_look_back < this->_count){
                return false;
            }
            if(count > 1 + this->_look_ahead + this->_look_back + this->_count){
                return false;
            }

            size_t actual_offset = count - this->_count;
            if(actual_offset >= this->_queue.size()){
                return false;
            }

            return true;
        }
    }

    std::vector<data>
    QueueingDataProxy::window(int offset_from, int offset_to)
    {
        if(this->_is_static){
            throw RuntimeError("QueueingDataProxy: Requested windowed data for static data-proxy");

        }else{
            UniqueLock lock(this->_mutex.get());
            //std::unique_lock<std::mutex>lock(this->_mutex);

            if(offset_from >= offset_to){
                throw RuntimeError("QueueingDataProxy: Invalid window index");
            }
            if(offset_from < 0 && (size_t)(-offset_from) > this->_look_back){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data beyond than look-back window";
                throw RuntimeError(ss.str());
            }
            if(offset_to > 0 && (size_t)offset_to > this->_look_ahead){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data beyond look-ahead window";
                throw RuntimeError(ss.str());
            }

            size_t actual_offset_from = offset_from + this->_look_back;
            size_t actual_offset_to   = offset_to + this->_look_back;
            if(actual_offset_from >= this->_queue.size()){
                std::stringstream ss;
                ss << "QueueingDataProxy: Requested data not ready";
                throw RuntimeError(ss.str());
            }

            return std::vector<data>(this->_queue.begin() + offset_from, this->_queue.begin() + offset_to);
        }
    }

    data
    QueueingDataProxy::pop()
    {
        if(this->_is_static){
            return data(this->_static_value);

        }else{
            if(this->_queue.empty()){
                std::stringstream ss;
                ss << "QueueingDataProxy: Tried to remove from empty queue";
                throw RuntimeError(ss.str());
            }

            UniqueLock lock(this->_mutex.get());
            //std::unique_lock<std::mutex>lock(this->_mutex);
            data packet = this->_queue.front();
            if(this->_queue.size() > this->_look_ahead + this->_look_back + queue_margin
               && this->_count - this->_head_cursor >= this->_look_back){
                this->_queue.pop_front();
                this->_head_cursor++;
#if defined(VERBOSE_DEBUG)
                {
                    std::stringstream ss;
                    ss << "QueueingDataProxy::pop: name=" << this->DataSource::_parent->name() << ", " << this->DataSink::name() << ", queue-size="<< this->_queue.size() << ", head_cursor=" << this->_head_cursor << std::endl;
                    std::cout << ss.str();
                }
#endif
            }
            this->_count++;
            //  std::cout << "QueueingDataProxy[" << this->DataSink::_parent->name() << "]: pop: count <- " << this->_count << std::endl;

            if(this->_up_stream){
                this->_up_stream->notify_send_data();
            }
            // if(this->_up_stream){
            //     BaseNode *node = this->_up_stream->parent();
            //     if(node){
            //         node->notify_queue_vacancy();
            //     }
            // // }else{
            // //     std::stringstream ss;
            // //     ss << "QueueingDataProxy: Has no upstream data source";
            // //     throw RuntimeError(ss.str());
            // }
            return packet;
        }
    }

    void
    QueueingDataProxy::push(const data &packet)
    {
#if defined(VERBOSE_DEBUG)
       {
           std::stringstream ss;
           ss << "QueueingDataProxy::push: name=" << this->DataSource::_parent->name() << std::endl;
           std::cout << ss.str();
       }
#endif
        UniqueLock lock(this->_mutex.get());
        //std::unique_lock<std::mutex>lock(this->_mutex);
        if(this->_queue.size() > this->_look_ahead + this->_look_back + this->_extended_window + this->_queue_margin + queue_margin){
            if(this->_is_synchronous){
                throw RuntimeError("QueueingDataProxy: Queue is full");
            }else{
                this->_queue.pop_front();
            }
        }
        this->_queue.push_back(packet);
        this->_condvar->notify_one();

        this->notify_receive_data();
        // BaseNode *node = this->DataSource::_parent;
        // if(node){
        //     node->notify_data_arrival();
        // }else{
        //     for(auto it: this->_down_streams){
        //         it->notify_receive_data();
        //     }
        // }
    }

    void
    QueueingDataProxy::notify_send_data()
    {
        BaseNode *node = this->DataSink::_parent;
        if(node){
            node->notify_queue_vacancy();
        }else if(this->_up_stream){
            this->_up_stream->notify_send_data();
        }
    }

    void
    QueueingDataProxy::notify_receive_data()
    {
        BaseNode *node = this->DataSource::_parent;
        if(node){
            node->notify_data_arrival();
        }else{
            for(auto it: this->_down_streams){
                it->notify_receive_data();
            }
        }
    }

    void
    QueueingDataProxy::set_static(const data &value)
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "QueueingDataProxy::set_static: "
                << ", node=" << this->DataSink::_parent->name()
                << ", name=" << this->DataSink::name()
                << ", value.is_empty=" << value.is_empty()
                << ", value=" << value
                << std::endl;
            std::cout << ss.str();
        }
#endif
        this->_static_value = value;
        this->_is_static = !value.is_empty();
    }

    void
    QueueingDataProxy::connect_from(DataSource* source)
    {
        this->DataSink::connect_from(source);
        this->_is_static = false;
    }

    bool
    QueueingDataProxy::is_synchronous() const
    {
        return this->_is_synchronous;
    }

    bool
    QueueingDataProxy::is_static() const
    {
        return this->_is_static;
    }

    bool
    QueueingDataProxy::is_source_ready() const
    {
        UniqueLock lock(this->_mutex.get());
        //std::unique_lock<std::mutex>lock(this->_mutex);
        bool is_ready = this->_is_static
            || (
                   (this->_head_cursor + this->_queue.size() > this->_count + this->_look_ahead_required)
                && (this->_queue.size() > this->_look_back_required));
#if defined(VERBOSE_DEBUG)
    {
        std::stringstream ss;
        ss << "QueueingDataProxy: is_source_ready: is-ready=" << is_ready
            << ", node=" << this->DataSource::_parent->name()
            << ", name=" << this->DataSink::name()
            << ", is_static=" << this->_is_static
            << ", queue-size=" << this->_queue.size()
            << ", count=" << this->_count
            << ", head_cursor=" << this->_head_cursor
            << ", look_ahead=" << this->_look_ahead
            << ", look_back=" << this->_look_back
            << ", look_ahead_required=" << this->_look_ahead_required
            << std::endl;
        std::cout << ss.str();
    }
#endif
        return is_ready;
    }

    bool
    QueueingDataProxy::is_sink_ready() const
    {
        UniqueLock lock(this->_mutex.get());
        //std::unique_lock<std::mutex>lock(this->_mutex);
        bool is_ready = !this->_is_static && (!this->_is_synchronous || this->_queue.size() <= (this->_look_ahead + this->_look_back + this->_extended_window + this->_queue_margin + queue_margin));
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "QueueingDataProxy::is_sink_ready: is-ready=" << is_ready
                << ", node=" << this->DataSink::_parent->name()
                << ", name=" << this->DataSink::name()
                << ", queue-size=" << this->_queue.size()
                << ", look_window=(" << this->_look_ahead << ", " << this->_look_back << ")"
                << ", extended-window=" << this->_extended_window
                << ", queue-margin=(" << this->_queue_margin << ", " << queue_margin << ")"
                << ", is-static=" << this->_is_static
                << ", is-synchronous=" << this->_is_synchronous
                << ", ?=" << (this->_look_ahead + this->_look_back + this->_extended_window + this->_queue_margin + queue_margin)
                << std::endl;
            std::cout << ss.str();
        }
#endif
        return is_ready;
    }

    void
    QueueingDataProxy::set_look_window(size_t look_back, size_t look_ahead, size_t look_back_required, size_t look_ahead_required)
    {
        // std::cout << "QueueingDataProxy: set_window: look_back=" << look_back << ", look_ahead=" << look_ahead << std::endl;
        if(this->_look_back == look_back && this->_look_ahead == look_ahead
            && this->_look_back_required == look_back_required && this->_look_ahead_required == look_ahead_required){
                return;
        }

        this->_look_back = look_back;
        this->_look_ahead = look_ahead;
        this->_look_back_required = std::min(look_back, look_back_required);
        this->_look_ahead_required = std::min(look_ahead, look_ahead_required);

        BaseNode *node = this->DataSource::_parent;
        if(node){
            node->update_delay();
        }
        if(this->_up_stream){
            BaseNode *node = this->_up_stream->parent();
            if(node){
                node->notify_queue_vacancy();
            }
        }
    }

    void
    QueueingDataProxy::set_extended_window(size_t extended_window)
    {
        this->_extended_window = extended_window;
        if(this->_up_stream){
            BaseNode *node = this->_up_stream->parent();
            if(node){
                node->notify_queue_vacancy();
            }
        }
    }

    size_t
    QueueingDataProxy::get_delay() const
    {
        return this->_upstream_delay + this->_look_ahead;
    }

    void
    QueueingDataProxy::set_delay(size_t delay)
    {
        this->_upstream_delay = delay;

        BaseNode *node = this->DataSource::_parent;
        if(node){
            node->update_delay();
        }
        for(auto ds: this->_down_streams){
            ds->set_delay(delay);
        }
    }
}

/* end of file */
