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

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

namespace hark
{
namespace node
{
    ConstantLocalization::ConstantLocalization():
        NodeDef("ConstatntLocalization")
    {
        *this
            << Terminal::in("ANGLES").def(std::vector<float>())
            << Terminal::in("ELEVATIONS").def(std::vector<float>())
            << Terminal::in("POWER").def(std::vector<float>())
            << Terminal::in("MIN_ID").def(0)
            >> Terminal::out("SOURCES");
    }

    void
    ConstantLocalization::reset()
    {
    }

    void
    ConstantLocalization::initialize(InputDataBundle &input)
    {
        this->angles = input["ANGLES"].last().cast<std::vector<float> >();
        this->elevations = input["ELEVATIONS"].last().cast<std::vector<float> >();

        data power_data = input["POWER"].last();
        if(power_data.is_type<std::vector<float> >()){
            this->power = power_data.cast<std::vector<float> >();
        }
        else if(power_data.is_type<float>()){
            this->power.resize(this->angles.size());
            for(size_t i = 0; i < this->power.size(); i++){
                this->power[i] = power_data.as<float>();
            }
        }
        else{
            throw RuntimeError("ConstantLocalization: Unexpected \"POWER\" type.");
        }

        if(this->angles.size() != this->elevations.size()){
            throw RuntimeError("ConstantLocalization: The vector size of the \"ANGLES\" and \"ELEVATIONS\" parameters are different.");
        }
        if(this->angles.size() != this->power.size()){
            throw RuntimeError("ConstantLocalization: The vector size of the \"ANGLES\" and \"POWER\" parameters are different.");
        }

        this->min_id = input["MIN_ID"].last().as<int>();
    }

    bool
    ConstantLocalization::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
#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> >();

        for(size_t i = 0; i < this->angles.size(); i++){
            float theta = this->angles[i] * this->coef;
            float phi = this->elevations[i] * this->coef;
            float cos_phi = std::cos(phi);

            Source src;
            src.id = this->min_id + i;
            src.x[0] = std::cos(theta) * cos_phi;
            src.x[1] = std::sin(theta) * cos_phi;
            src.x[2] = std::sin(phi);
            src.power = this->power[i];
            src.compare_mode = Source::CompareMode::DEG;

            output_sources.push_back(data(std::move(src)));
        }

        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 */
