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

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

namespace hark
{
namespace node
{
    SourceTracker::SourceTracker():
        NodeDef("SourceTracker")
    {
        *this
            << Terminal::in("INPUT")
            << Terminal::in("THRESH").def(20.0f)
            << Terminal::in("PAUSE_LENGTH").def(800.0f)
            << Terminal::in("MIN_SRC_INTERVAL").def(20.0f)
            << Terminal::in("MIN_ID").def(0)
            << Terminal::in("DEBUG").def(false)
            >> Terminal::out("OUTPUT");
    }

    void
    SourceTracker::reset()
    {
    }

    void
    SourceTracker::initialize(InputDataBundle &input)
    {
        this->compare_mode = Source::CompareMode::DEG;
        this->thresh = input["THRESH"].last().as<float>();
        this->pause_length = input["PAUSE_LENGTH"].last().as<float>();
        this->min_source_interval = input["MIN_SRC_INTERVAL"].last().as<float>();
        this->min_source_interval_rad = this->min_source_interval * this->coef_deg2rad;
        this->min_tfindex_interval = 1.0f;
        this->next_id = input["MIN_ID"].last().as<int>();
        this->debug = input["DEBUG"].last().as<bool>();

        if(this->next_id < 0){
            std::stringstream ss;
            ss << "SourceTracker: MIN_ID should be greater than 0 or equal. MIN_ID = " << this->next_id << "\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
    }

    bool
    SourceTracker::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
        // for debug
        /*
        data in = input["INPUT"].last();
        if(in.is_type<std::vector<Source> >()){
            throw RuntimeError("The previous node output was std::vector<Source>, but the correct output is std::vector<data>. This may be a bug!");
        }
        else if(in.is_type<std::vector<data> >()){
            const auto &in_vec = in.cast<std::vector<data> >();
            for(std::vector<data>::iterator it = in_vec.begin(); it != in_vec.end(); ++it){
                if(it->is_type<Source>()){ } // pass
                else{
                    std::cout << "std::vector<data>'s data TYPE: " << it->get_type().name() << std::endl;
                }
            }
        }
        else{
            std::cout << "INPUT TYPE: " << in.get_type().name() << std::endl;
        }
        */

        const auto &input_sources = input["INPUT"].last().cast<std::vector<data> >();

#ifdef DEBUG
        ptime start_time;
        ptime end_time;
        start_time = microsec_clock::local_time();
#endif

        if(count == 0){ this->initialize(input); }

        auto out = data::create<std::vector<data> >();
        auto &output_sources = out.cast<std::vector<data> >();

        std::vector<data> remaining_sources(input_sources);

        for(size_t i = 0; i < this->last_sources.size(); i++){
            if(this->last_sources[i].cast<Source>().remaining < 0){ continue; } // expired source
            if(this->compare_mode == Source::CompareMode::DEG){
                float min_angle = std::numeric_limits<float>::max();
                size_t min_id = 0;
                for(size_t j = 0; j < remaining_sources.size(); j++){
                    if(remaining_sources[j].cast<Source>().power < this->thresh){ continue; }
                    float angle = remaining_sources[j].cast<Source>().angle(this->last_sources[i].cast<Source>().x);
                    if(angle < min_angle){ min_angle = angle; min_id = j; }
                }
                if(min_angle < this->min_source_interval_rad){
                    Source src = remaining_sources[min_id].cast<Source>();
                    src.id = this->last_sources[i].cast<Source>().id;
                    src.remaining = this->pause_length;
                    remaining_sources.erase(remaining_sources.begin() + min_id);
                    output_sources.push_back(data(src));
                }
                else{
                    Source src = this->last_sources[i].cast<Source>();
                    src.remaining -= 10.0f;
                    output_sources.push_back(data(src));
                }
            }
            else{ // TFINDEX
                int min_distance = std::numeric_limits<int>::max();
                size_t min_id = 0;
                for(size_t j = 0; j < remaining_sources.size(); j++){
                    if(remaining_sources[j].cast<Source>().power < this->thresh){ continue; }
                    int distance = remaining_sources[j].cast<Source>().tfindexDistance(this->last_sources[i].cast<Source>().tfindex);
                    if(distance < min_distance){ min_distance = distance; min_id = j; }
                }
                if(min_distance < this->min_tfindex_interval){
                    Source src = remaining_sources[min_id].cast<Source>();
                    src.id = this->last_sources[i].cast<Source>().id;
                    src.remaining = this->pause_length;
                    remaining_sources.erase(remaining_sources.begin() + min_id);
                    output_sources.push_back(data(src));
                }
                else{
                    Source src = this->last_sources[i].cast<Source>();
                    src.remaining -= 10.0f;
                    output_sources.push_back(data(src));
                }
            }
        }
        for(size_t i = 0; i < input_sources.size(); i++){
            if(input_sources[i].cast<Source>().power < this->thresh){ continue; }
            if(this->compare_mode == Source::CompareMode::DEG){
                float min_angle = std::numeric_limits<float>::max();
                size_t min_id = 0;
                for(size_t j = 0; j < output_sources.size(); j++){
                    float angle = output_sources[j].cast<Source>().angle(input_sources[i].cast<Source>().x);
                    if(angle < min_angle){ min_angle = angle; min_id = j; }
                }
                if(min_angle >= this->min_source_interval_rad){
                    Source src = input_sources[i].cast<Source>();
                    src.id = this->next_id++;
                    src.remaining = this->pause_length;
                    output_sources.push_back(data(src));
                }
            }
            else{ // TFINDEX
                int min_distance = std::numeric_limits<int>::max();
                size_t min_id = 0;
                for(size_t j = 0; j < output_sources.size(); j++){
                    int distance = output_sources[j].cast<Source>().tfindexDistance(input_sources[i].cast<Source>().tfindex);
                    if(distance < min_distance){ min_distance = distance; min_id = j; }
                }
                if(min_distance >= this->min_tfindex_interval){
                    Source src = input_sources[i].cast<Source>();
                    src.id = this->next_id++;
                    src.remaining = this->pause_length;
                    output_sources.push_back(data(src));
                }
            }
        }

        if(this->debug){
            std::stringstream ss;
            ss << "{count: " << count << ", output_sources: [";
            for(size_t i = 0; i < output_sources.size(); i++){
                if(i != 0){ ss << ", "; }
                ss << output_sources[i].cast<Source>();
            }
            ss << "]}";
            std::cout << ss.str() << std::endl;
        }

        last_sources = output_sources;

        output["OUTPUT"].push(out);

#ifdef DEBUG
        end_time = microsec_clock::local_time();
        std::cout << end_time - start_time << std::endl;
#endif

        return true;
    }
}
}

/* end of file */
