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

#include "hark-lib/dispatch.h"

#include "hark-lib/node.h"
#include "hark-lib/internal/thread.h"
#include "hark-lib/internal/mutex.h"
#include "hark-lib/internal/condition_variable.h"
#include "hark-lib/internal/lock.h"
 //#include <unistd.h>

#include <algorithm>
#include <condition_variable>

namespace hark
{
namespace dispatch
{

    Scheduler::Scheduler():
        _to_quit(false),
        _mutex(new Mutex()),
        _condvar(new ConditionVariable())
    {
    }

    Scheduler::~Scheduler()
    {
    }

    void
    Scheduler::initialize()
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "Scheduler::initialize(), this=" << (void *)this << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        // for(auto &p: this->_dispatchers){
        //     delete p;
        // }
        for(auto &p: this->_dispatcher_threads){
            delete p;
        }
        this->_dispatchers.clear();
        this->_dispatcher_threads.clear();
        this->_dispatcher_exceptions.clear();
    }

    void
    Scheduler::add_dispatcher(BaseDispatcher *dispatcher)
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "Scheduler::add_dispatcher" << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        // this->_dispatchers.push_back(std::unique_ptr<BaseDispatcher>(dispatcher));
        this->_dispatchers.push_back(dispatcher);
    }

    void
    Scheduler::start_dispatchers()
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "Scheduler::start_dispatchers(): dispatcher-count=" << this->_dispatchers.size() << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        this->_to_quit = false;
        // invoke dispatcher threads
        for(auto &dispatcher: this->_dispatchers){
            // std::cout << "Scheduler::start_dispatchers(): 1: dispatcher=" << (void *)dispatcher << ", this=" << (void *)this << std::endl;
            dispatcher->initialize(*this);
        }
        for(auto &dispatcher: this->_dispatchers){
            // std::cout << "Scheduler::start_dispatchers(): 2: dispatcher=" << (void *)dispatcher << ", this=" << (void *)this << std::endl;
            //this->_dispatcher_threads.push_back(new std::thread(std::ref(*dispatcher)));
            this->_dispatcher_threads.push_back(new Thread(std::ref(*dispatcher)));
            // while(!dispatcher->is_started());
            // std::cout << "Scheduler::start_dispatchers(): 3: dispatcher=" << (void *)dispatcher << ", this=" << (void *)this << std::endl;
        }
    }

    void
    Scheduler::wait_for_idle()
    {
        UniqueLock lock(this->_mutex.get());
        this->_condvar->wait(lock, [this](){
            size_t c = std::count_if(this->_dispatchers.begin(), this->_dispatchers.end(),
                                       [](BaseDispatcher *dispatcher){
                    return !dispatcher->is_idle();
                });
            // std::cout << "Scheduler::wait_for_idle: c=" << c << std::endl;
            return c == 0;
        });

        if(this->_dispatcher_exceptions.empty()){
            return;

        }else{
            throw this->_dispatcher_exceptions.front();
        }
    }

    void
    Scheduler::request_quit(bool to_quit)
    {
        // std::unique_lock<std::mutex> lock(this->_mutex);
        if(this->_to_quit != to_quit){
            this->_to_quit = to_quit;
            for(auto &dispatcher: this->_dispatchers){
                dispatcher->notify();
            }
        }
    }

    bool
    Scheduler::to_quit() const
    {
        return this->_to_quit;
    }

    void
    Scheduler::notify()
    {
        UniqueLock lock(this->_mutex.get());
        // std::cout << ">>>> notify" << std::endl;
        this->_condvar->notify_one();
    }

    void
    Scheduler::join()
    {
        for(auto v: this->_dispatcher_threads){
            v->join();
            delete v;
        }
        this->_dispatcher_threads.clear();
    }

    void
    Scheduler::handle_exception(std::exception &ex)
    {
        {
            UniqueLock lock(this->_mutex.get());
            // std::cerr << "Scheduler::handle_exception: " << ex.what() << std::endl;
            RuntimeError *re = dynamic_cast<RuntimeError *>(&ex);
            if(re){
                this->_dispatcher_exceptions.push_back(*re);
            }else{
                std::stringstream ss;
                ss << "Exception in process: " << ex.what();
                this->_dispatcher_exceptions.push_back(RuntimeError(ss.str()));
            }
        }
        this->request_quit();
    }

    BaseDispatcher::BaseDispatcher():
        _scheduler(nullptr), _status(Start), _triggered(false), _loop_count(0), _pred_count(0), _loop_condition(true),
        _mutex(new Mutex()),
        _condvar(new ConditionVariable()),
        _loop_condition_datasink(new FunctionalDataSink([this](const data &v){
            this->_loop_condition = v.cast<bool>();
        }))
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "BaseDispatcher::ctor: " << (void *)this << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
    }

    BaseDispatcher::~BaseDispatcher()
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "BaseDispatcher::dtor: " << (void *)this << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        // std::cout << "BaseDispatcher::dtor: " << (void *)this << std::endl;
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "pred_count: " << this->_pred_count << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
    }

    void
    BaseDispatcher::set_loop_condition(DataSource &source)
    {
        source.connect_to(this->_loop_condition_datasink.get());
    }

    bool
    BaseDispatcher::is_idle() const
    {
        auto st = this->_status;
        bool sw = (st == Wait);
        bool sf = (st == Finished);
        bool t = !this->_triggered;
        // std::cout << "BaseDispatcher: is_idle: s=" << s << "(" << st << "), t=" << t << std::endl;
        return sw && t || sf;
    }

    void
    BaseDispatcher::notify_data_arrival()
    {
        this->hook_data_arrival();
        // this->notify();
    }

    void
    BaseDispatcher::notify_queue_vacancy()
    {
        this->hook_queue_vacancy();
        // this->notify();
    }

    void
    BaseDispatcher::notify()
    {
        UniqueLock lock(this->_mutex.get());
        //std::unique_lock<std::mutex>lock(this->_mutex);
        // std::cout << "BaseDispatcher::notify" << std::endl;
        this->_condvar->notify_one();
    }

    void
    BaseDispatcher::trigger()
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "BaseDispatcher::trigger: " << this->_nodes[0]->name() << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        UniqueLock lock(this->_mutex.get());
        //std::unique_lock<std::mutex>lock(this->_mutex);
        this->_triggered = true;
        this->_condvar->notify_one();
    }

    void
    BaseDispatcher::add_node(BaseNode *node)
    {
        this->_nodes.push_back(node);
        // node->assign_dispatcher(this);
    }

    void
    BaseDispatcher::initialize(Scheduler &scheduler)
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "BaseDispatcher::initialize: node=" << this->_nodes[0]->name() << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */
        this->_scheduler = &scheduler;
        this->_status = Start;
        this->_triggered = false;
    }

    void
    BaseDispatcher::operator () () // (DataDispatcherControl &dataDispatcherControl)
    {
#if defined(VERBOSE_DEBUG)
        {
            std::stringstream ss;
            ss << "BaseDispatcher::operator () (): node=" << this->_nodes[0]->name() << std::endl;
            std::cout << ss.str();
        }
#endif /* #if defined(VERBOSE_DEBUG) */

        this->_loop_count = 0;

        this->hook_initial();
        while(!this->_scheduler->to_quit()){
#if defined(VERBOSE_DEBUG)
            {
                std::stringstream ss;
                ss << "dispatcher: node=" << this->_nodes[0]->name() << ", trigger=" << this->_triggered << std::endl;
                std::cout << ss.str();
            }
#endif /* #if defined(VERBOSE_DEBUG) */
            this->_status = Running;
            this->_scheduler->notify();

            // DataPacket packet;
            bool triggered = false;
            {
                UniqueLock lock(this->_mutex.get());
                //std::unique_lock<std::mutex>lock(this->_mutex);
                if(!this->_scheduler->to_quit() && !this->_triggered){
                    this->_status = Wait;
                    this->_scheduler->notify();
                    do {
                        this->_pred_count++;
                        if(this->_triggered && std::count_if(this->_nodes.begin(), this->_nodes.end(), [](BaseNode *node){
                                return node->is_ready();
                            }) > 0){
                            break;
                        }
                        this->_condvar->wait(lock);
#if defined(VERBOSE_DEBUG)
                        {
                            std::stringstream ss;
                            ss << "dispatcher: node=" << this->_nodes[0]->name() << ", awaken" << std::endl;
                            std::cout << ss.str();
                        }
#endif /* #if defined(VERBOSE_DEBUG) */
                    } while(!this->_scheduler->to_quit() && !this->_triggered);
                }
                this->_status = Running;
                this->_scheduler->notify();

                // packet = this->_queue.shift();
                triggered = this->_triggered;
                this->_triggered = false;
#if defined(VERBOSE_DEBUG)
                {
                    std::stringstream ss;
                    ss << "node: " << this->_nodes[0]->name() << ", triggered: " << triggered << std::endl;
                    std::cout << ss.str();
                }
#endif /* #if defined(VERBOSE_DEBUG) */
            }

            if(this->_scheduler->to_quit()){
                break;

            }else if(triggered){
                this->hook_start();

                bool processed = false;
                for(auto &node: this->_nodes){
                    try{
                        // std::cout << "## " << this->_nodes[0]->name() << ", " << this->_nodes.size() << std::endl;
#if defined(VERBOSE_DEBUG)
                       {
                           std::stringstream ss;
                           ss << "node: " << this->_nodes[0]->name() << ", try-process" << std::endl;
                           std::cout << ss.str();
                       }
#endif /* #if defined(VERBOSE_DEBUG) */
                        bool r = false;

                        try{
                            r = node->try_process();
                        }catch(std::exception &ex){
                            std::stringstream ss;
                            ss << "Exception in " << node->get_nodedef()->name() << "(" << node->name() << "): " << ex.what();
                            RuntimeError err(ss.str());
                            this->_scheduler->handle_exception(err);
                            break;
                        }

                        if(r){
                            processed = true;
                        }
#if defined(VERBOSE_DEBUG)
                       {
                           std::stringstream ss;
                           ss << "node: " << this->_nodes[0]->name() << ", try-process: r=" << r << std::endl;
                           std::cout << ss.str();
                       }
#endif /* #if defined(VERBOSE_DEBUG) */
                    }catch(RuntimeError &ex){
                        std::stringstream ss;
                        ss << "Exception thrown in node process: node=" << node->name() << ": " << ex.what() << std::endl;
                        std::cerr << ss.str();
                        break;
                    }
                }

                this->hook_end(processed);
                if(processed){
                    this->_loop_count++;
                }
            }
        }

        this->hook_final();

        this->_status = Finished;
        this->_scheduler->notify();
    }

    void
    BaseDispatcher::hook_initial()
    {
    }

    void
    BaseDispatcher::hook_start()
    {
    }

    void
    BaseDispatcher::hook_end(bool processed)
    {
    }

    void
    BaseDispatcher::hook_final()
    {
    }

    void
    BaseDispatcher::hook_data_arrival()
    {
        this->trigger();
    }

    void
    BaseDispatcher::hook_queue_vacancy()
    {
        this->trigger();
    }

    void
    OneShot::hook_initial()
    {
        this->trigger();
    }
    
    bool
    OneShot::is_synchronous() const
    {
        return false;
    }
    
    void
    OneShot::hook_data_arrival()
    {
    }
    
    void
    OneShot::hook_queue_vacancy()
    {
    }

    // 他のノードの状態にかかわらず、繰り返し実行される
    // ただし、loop_condition が false になるか、実行メソッドが false を返すと終了する
    void
    Repeat::hook_initial()
    {
        this->trigger();
    }

    bool
    Repeat::is_synchronous() const
    {
        return false;
    }

    void
    Repeat::hook_end(bool processed)
    {
        if(processed && this->_loop_condition) this->trigger();
    }

    // 他のノードの状態にかかわらず、繰り返し実行される
    // ただし、loop_condition が false になるか、実行メソッドが false を返すと終了する
    void
    TriggeredRepeat::hook_end(bool processed)
    {
        if(processed && this->_loop_condition) this->trigger();
    }
    
    bool
    TriggeredRepeat::is_synchronous() const
    {
        return false;
    }

    // 上流からのデータ到達をきっかけとして、繰り返し実行される
    // 実行メソッドが true を返すと、入力バッファは一つ値を削る
    void
    TriggeredMultiShot::hook_end(bool processed)
    {
        if(processed){
            this->trigger();
        }
    }

    bool
    TriggeredMultiShot::is_synchronous() const
    {
        return true;
    }

    bool
    TriggeredOneShot::is_synchronous() const
    {
        return true;
    }

}
}

/* end of file */
