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

#include "hark-lib/node/Sleep.h"

namespace hark
{
namespace node
{
    Sleep::Sleep():
        NodeDef("Sleep")
    {
        *this
            << Terminal::in("INPUT")
            << Terminal::in("SECONDS").def(0.01f)
            << Terminal::in("INTERRUPT_CYCLE").def(10)
            << Terminal::in("USE_INPUT_OUTPUT").def(false)
            >> Terminal::out("OUTPUT")
            >> Terminal::out("VALUE");
        this->start = std::chrono::high_resolution_clock::now();
        this->loop_continue.store(true);
        this->stay_count.store(0);
        this->timer_th = nullptr;
    }

    Sleep::~Sleep()
    {
        this->self.reset(); // To safely use "this" in a lambda function in another thread.

        this->loop_continue.store(false);
        if(this->timer_th != nullptr){
            timer_th->detach();
            delete timer_th;
            timer_th = nullptr;
        }
    }

    void
    Sleep::reset()
    {
    }

    void
    Sleep::initialize(InputDataBundle &input)
    {
        this->self = shared_from_this(); // To safely use "this" in a lambda function in another thread.

        this->interrupt_cycle = std::chrono::milliseconds(input["INTERRUPT_CYCLE"].last().as<int>());
        this->use_input_output = input["USE_INPUT_OUTPUT"].last().as<bool>();

        this->seconds = input["SECONDS"].last().as<float>();

/*
        this->timer_th = new std::thread([this]{
            std::chrono::high_resolution_clock::time_point _expect = this->start;
            const std::chrono::microseconds timer_interval((this->seconds * 1000000 < 100.0f) ? 100 : static_cast<int64_t>(this->seconds * 1000000));
            size_t local_count = 0;
            while(this->loop_countinue.load()){
                std::chrono::high_resolution_clock::time_point current = std::chrono::high_resolution_clock::now();
                if(current < _expect){ std::this_thread::sleep_for(_expect - current); }
                local_count++;
                _expect += time_interval;
                this->stay_count.store(local_count);
            }
        }
*/

        // To safely use "this" in a lambda function in another thread.
        //
        // lambda capture initializers only available with ‘-std=c++14’ or ‘-std=gnu++14’
        //this->timer_th = new std::thread([weak_this = std::weak_ptr<Sleep>(this->self)]{
        this->timer_th = new std::thread([this]{
            std::weak_ptr<Sleep> weak_this(this->self);
            std::chrono::high_resolution_clock::time_point _expect;
            float _seconds = 0.0f;
            if(auto safe_this = weak_this.lock()){
                _expect = safe_this->start;
                _seconds = safe_this->seconds;
            }
            const std::chrono::microseconds timer_interval((_seconds * 1000000 < 100.0f) ? 100 : static_cast<int64_t>(_seconds * 1000000));
            size_t local_count = 0;
            while(!weak_this.expired() && weak_this.lock()->loop_continue.load()){
                std::chrono::high_resolution_clock::time_point current = std::chrono::high_resolution_clock::now();
                if(current < _expect){ std::this_thread::sleep_for(_expect - current); }
                local_count++;
                _expect += timer_interval;
                if(!weak_this.expired()){ weak_this.lock()->stay_count.store(local_count); }
            }
        });
    }

    bool
    Sleep::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
        while(count >= this->stay_count.load()){
            std::this_thread::sleep_for(this->interrupt_cycle);
        }

        data out;
        if(use_input_output){ out = input["INPUT"].at(count); }

        output["VALUE"].push(data::create<bool>(true));
        output["OUTPUT"].push(out);

        return true;
    }
}
}

/* end of file */
