
#include <cmath>
#include <complex>
#include <algorithm>
#include <iostream>

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

namespace hark
{
namespace node
{
    GHDSS::GHDSS():
        NodeDef("GHDSS")
    {
        *this
            << Terminal::in("INPUT_FRAMES")
            << Terminal::in("INPUT_SOURCES")
            << Terminal::in("TRANSFER_FUNCTION").def(false)
            << Terminal::in("LENGTH").def(512)
            << Terminal::in("ADVANCE").def(160)
            << Terminal::in("SAMPLING_RATE").def(16000)
            << Terminal::in("LOWER_BOUND_FREQUENCY").def(0)
            << Terminal::in("UPPER_BOUND_FREQUENCY").def(8000)
            << Terminal::in("TF_INPUT_TYPE").def("FILE")
            << Terminal::in("TF_CONJ_FILENAME").def("")
            << Terminal::in("INITW_FILENAME").def("")
            << Terminal::in("SS_METHOD").def("ADAPTIVE")
            << Terminal::in("SS_SCAL").def(1.0f)
            << Terminal::in("SS_MYU").def(0.001f)
            << Terminal::in("NOISE_FLOOR").def(0.0f)
            << Terminal::in("LC_CONST").def("FULL")
            << Terminal::in("LC_METHOD").def("ADAPTIVE")
            << Terminal::in("LC_MYU").def(0.001f)
            << Terminal::in("UPDATE_METHOD_TF_CONJ").def("POS")
            << Terminal::in("UPDATE_METHOD_W").def("ID_POS")
            << Terminal::in("UPDATE_ACCEPT_DISTANCE").def(300.0f)
            << Terminal::in("EXPORT_W").def(false)
            << Terminal::in("EXPORT_W_FILENAME").def("")
            << Terminal::in("UPDATE").def("STEP")
            >> Terminal::out("OUTPUTW")
            >> Terminal::out("OUTPUT");
    }

    void
    GHDSS::reset()
    {
    }

    void
    GHDSS::initialize(InputDataBundle &input)
    {
        this->ghdss_exec.length = input["LENGTH"].last().as<int>();
        if(this->ghdss_exec.length <= 0){
            std::stringstream ss;
            ss << "GHDSS: LENGTH is not invalid value.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.sampling_rate = input["SAMPLING_RATE"].last().as<int>();
        if(this->ghdss_exec.sampling_rate <= 0){
            std::stringstream ss;
            ss << "GHDSS: SAMPLING_RATE is not invalid value.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.advance = input["ADVANCE"].last().as<int>();

        this->lower_bound = input["LOWER_BOUND_FREQUENCY"].last().as<int>();
        this->upper_bound = input["UPPER_BOUND_FREQUENCY"].last().as<int>();


        this->lower_bin = static_cast<int>(this->lower_bound * this->ghdss_exec.length / this->ghdss_exec.sampling_rate);
        this->upper_bin = static_cast<int>(this->upper_bound * this->ghdss_exec.length / this->ghdss_exec.sampling_rate);

        std::string tf_input_type = input["TF_INPUT_TYPE"].last().as<std::string>();
        if(tf_input_type == "FILE"){
            this->ghdss_exec.tf_type = GHDSS_exec::TF_INPUT_TYPE::FILE;
            this->ghdss_exec.filename_tf = input["TF_CONJ_FILENAME"].last().cast<std::string>();
            // file existence check. if not exists, thow exception.
        }
        else if(tf_input_type == "ONLINE"){
            this->ghdss_exec.tf_type = GHDSS_exec::TF_INPUT_TYPE::ONLINE;
            this->ghdss_exec.filename_tf = ""; // not used.
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: Unexpected TF_INPUT_TYPE is set.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.filename_sm = input["INITW_FILENAME"].last().cast<std::string>();
        if(this->ghdss_exec.filename_sm.empty()){
        }
        else{
        }

        std::string ss_method_str = input["SS_METHOD"].last().cast<std::string>();
        if(ss_method_str == "FIX"){
            this->ghdss_exec.ss_method = GHDSS_exec::SS_METHOD::FIX;
        }
        else if(ss_method_str == "LC_MYU"){
            this->ghdss_exec.ss_method = GHDSS_exec::SS_METHOD::LCMYU;
        }
        else if(ss_method_str == "ADAPTIVE"){
            this->ghdss_exec.ss_method = GHDSS_exec::SS_METHOD::ADAPTIVE;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: SS_METHOD should be FIX, LC_METHOD or ADAPTIVE\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.ss_scale = input["SS_SCAL"].last().as<float>();

        this->ghdss_exec.ss_myu = input["SS_MYU"].last().as<float>();

        this->ghdss_exec.noise_floor = input["NOISE_FLOOR"].last().as<float>();

        std::string lc_const_str = input["LC_CONST"].last().cast<std::string>();
        if(lc_const_str == "FULL"){
            this->ghdss_exec.lc_const = GHDSS_exec::LC_CONST::FULL;
        }
        else if(lc_const_str == "DIAG"){
            this->ghdss_exec.lc_const = GHDSS_exec::LC_CONST::DIAG;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: LC_CONST should be FULL or DIAG\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        std::string lc_method_str = input["LC_METHOD"].last().cast<std::string>();
        if(lc_method_str == "FIX"){
            this->ghdss_exec.lc_method = GHDSS_exec::LC_METHOD::FIX;
        }
        else if(lc_method_str == "ADAPTIVE"){
            this->ghdss_exec.lc_method = GHDSS_exec::LC_METHOD::ADAPTIVE;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: LC_METHOD should be FIX or ADAPTIVE\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.lc_myu = input["LC_MYU"].last().as<float>();

        //this->ghdss_exec.w_myu = 0.0f; // Currently unused.

        std::string update_str = input["UPDATE"].last().cast<std::string>();
        if(update_str == "STEP"){
            this->ghdss_exec.update = GHDSS_exec::UPDATE_TYPE::STEP;
        }
        else if(update_str == "TOTAL"){
            this->ghdss_exec.update = GHDSS_exec::UPDATE_TYPE::TOTAL;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: UPDATE should be STEP or TOTAL\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        this->ghdss_exec.threshold = 1000.0f;

        std::string update_method_tf_str = input["UPDATE_METHOD_TF_CONJ"].last().cast<std::string>();
        if(update_method_tf_str == "ID"){
            this->ghdss_exec.update_method_tf = GHDSS_exec::UPDATE_METHOD::ID;
        }
        else if(update_method_tf_str == "POS"){
            this->ghdss_exec.update_method_tf = GHDSS_exec::UPDATE_METHOD::POS;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: UPDATE_TF_CONJ should be ID or POS.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        std::string update_method_w_str = input["UPDATE_METHOD_W"].last().cast<std::string>();
        if(update_method_w_str == "ID"){
            this->ghdss_exec.update_method_w = GHDSS_exec::UPDATE_METHOD::ID;
        }
        else if(update_method_w_str == "POS"){
            this->ghdss_exec.update_method_w = GHDSS_exec::UPDATE_METHOD::POS;
        }
        else if(update_method_w_str == "ID_POS"){
            this->ghdss_exec.update_method_w = GHDSS_exec::UPDATE_METHOD::ID_POS;
        }
        else{
            std::stringstream ss;
            ss << "GHDSS: UPDATE_W should be ID, POS or ID_POS.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        float distance = input["UPDATE_ACCEPT_DISTANCE"].last().as<float>();
        this->ghdss_exec.acceptable_distance = distance * 0.001f; // [mm] => [m]

        this->exportw = input["EXPORT_W"].last().as<bool>();
        if(this->exportw){
            this->exportw_filename = input["EXPORT_W_FILENAME"].last().cast<std::string>();
        }
        else{
            this->exportw_filename = "";
        }

        this->ghdss_exec.Initialize();

        this->req_init = true;
    }

    bool
    GHDSS::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
        ////const auto &operation_flag = input["OPERATION_FLAG"].last().as<bool>();
        const auto &input_frames = input["INPUT_FRAMES"].last().cast<hark::Matrix<std::complex<float> > >();
        const auto &input_sources = input["INPUT_SOURCES"].last().cast<std::vector<hark::data> >();

        hark::TransferFunction tfData;
        hark::data tf_input = input["TRANSFER_FUNCTION"].last();
        if(this->ghdss_exec.tf_type == GHDSS_exec::TF_INPUT_TYPE::ONLINE){
            if(tf_input.is_type<hark::TransferFunction>()){
                tfData = tf_input.cast<hark::TransferFunction>();
                this->ghdss_exec.SetTfData(tfData);
            }
        }
        else if(this->ghdss_exec.tf_type == GHDSS_exec::TF_INPUT_TYPE::FILE){
            if(tf_input.is_type<hark::TransferFunction>()){
                std::cerr << "GHDSS: [WARNING] If 'FILE' is selected in the 'TF_INPUT_TYPE' parameter, the input will be ignored." << std::endl;
            }
        }
        else{
        }

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

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

        auto out_sep = data::create<std::map<int, data> >();
        auto &sep = out_sep.cast<std::map<int, data> >();

        auto out_w = data::create<Tensor<std::complex<float> > >();
        auto &w = out_w.cast<Tensor<std::complex<float> > >();

        if(input_sources.size() > 0){
            std::vector<GHDSS_exec::Location> input_src_pos(input_sources.size());
            for(size_t i = 0; i < input_sources.size(); i++){
                hark::Source &src = input_sources[i].cast<hark::Source>();
                if(input_sources[i].is_type<hark::Source>()){
                    src = input_sources[i].cast<hark::Source>();
                    input_src_pos[i].x[0] = src.x[0];
                    input_src_pos[i].x[1] = src.x[1];
                    input_src_pos[i].x[2] = src.x[2];
                    input_src_pos[i].idpos = src.id;
                    input_src_pos[i].iddir = 0;
                    //input_src_pos[i].tfindex[0] = src.tfindex[0];
                    //input_src_pos[i].tfindex[1] = src.tfindex[1];
                    //input_src_pos[i].tfindex[2] = src.tfindex[2];
                    //input_src_pos[i].tfindex_max[0] = src.tfindex_max[0];
                    //input_src_pos[i].tfindex_max[1] = src.tfindex_max[1];
                    //input_src_pos[i].tfindex_max[2] = src.tfindex_max[2];
                    //input_src_pos[i].tfindex_min[0] = src.tfindex_min[0];
                    //input_src_pos[i].tfindex_min[1] = src.tfindex_min[1];
                    //input_src_pos[i].tfindex_min[2] = src.tfindex_min[2];
                    if(src.compare_mode == hark::Source::CompareMode::TFINDEX){
                        input_src_pos[i].tfpos = src.tfindex[0];
                    }
                    else{
                        input_src_pos[i].tfpos =
                            this->ghdss_exec.GetPosIdFromTFDBByCoord(input_src_pos[i]);
                    }
                }
            }

            // this->src_pos is sorted input_src_pos by idpos.
            this->src_pos.resize(input_src_pos.size());
            std::copy(input_src_pos.begin(), input_src_pos.end(), this->src_pos.begin());
            std::sort(this->src_pos.begin(), this->src_pos.end(), CompareLocations());

            // In the original code, it is initialized in the total frequency area.
            // .ha_iLowerFreq and .ha_iUpperFreq simply save the parameters.
            // 
            //this->InputSpec.resize(input_frames.rows(), this->lower_bin, this->upper_bin);

            // Added copy constructor that passed hark::Matrix to hark::FreqBins
            //this->InputSpec.resize(input_frames.rows(), 0, input_frames.cols());
            //for(size_t i = 0; i < input_frames.rows(); i++){
            //    for(size_t j = 0; j < input_frames.cols(); j++){
            //        this->InputSpec.bins(i, j) = input_frames(i, j);
            //    }
            //}

            this->InputSpec = hark::FreqBins<std::complex<float> >(input_frames);

            if(this->req_init){
                ////DEBUG////std::cout << "if this->req_init == true" << std::endl;
                ////DEBUG////std::cout << "input_frames.rows()=" << input_frames.rows() << std::endl;
                ////DEBUG////std::cout << "input_frames.cols()=" << input_frames.cols() << std::endl;
                ////DEBUG////std::cout << "src_pos.size()=" << src_pos.size() << std::endl;
                this->ghdss_exec.Initialize_Change(input_frames.rows(), this->src_pos.size(), 0, input_frames.cols());
                ////DEBUG////std::cout << "executed Initialize_Change()" << std::endl;
                this->ghdss_exec.Initialize_GHDSS(input_frames.rows(), this->src_pos.size(), 0, input_frames.cols());
                ////DEBUG////std::cout << "executed Initialize_GHDSS()" << std::endl;
                if(this->ghdss_exec.AssignSrcPos(this->src_pos) == 0){
                    std::stringstream ss;
                    ss << "GHDSS: Fail to assign source position.\n"
                       << "Exception caught in .AssignSrcPos() "
                       << __FILE__ << ", " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                ////DEBUG////std::cout << "executed AssignSrcPos()" << std::endl;
                //if(this->ghdss_exec.InitTfMatrix(this->ghdss_frame_params) == 0){
                if(this->ghdss_exec.InitTFMatrix() == 0){
                    std::stringstream ss;
                    ss << "GHDSS: Error initialize TF matrix.\n"
                       << "Exception caught in .InitTfMatrix() "
                       << __FILE__ << ", " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                ////DEBUG////std::cout << "executed InitTFMatrix()" << std::endl;
                //if(this->ghdss_exec.InitSepMatrix(this->ghdss_frame_params) == 0){
                if(this->ghdss_exec.InitSepMatrix() == 0){
                    std::stringstream ss;
                    ss << "GHDSS: Error initialize Separate matrix.\n"
                       << "Exception caught in .InitSepMatrix() "
                       << __FILE__ << ", " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                ////DEBUG////std::cout << "executed InitSepMatrix()" << std::endl;
                if(this->ghdss_exec.InitUpdateW() == 0){
                    std::stringstream ss;
                    ss << "GHDSS: Initialize Update W matrix.\n"
                       << "Exception caught in .InitUpdateW() "
                       << __FILE__ << ", " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                ////DEBUG////std::cout << "executed InitUpdateW()" << std::endl;
                ////DEBUG////std::cout << "set this->req_init = false" << std::endl;
                this->req_init = false;
            }
            else{
                ////DEBUG////std::cout << "if this->req_init == false" << std::endl;
                ////DEBUG////std::cout << "input_frames.rows()=" << input_frames.rows() << std::endl;
                ////DEBUG////std::cout << "input_frames.cols()=" << input_frames.cols() << std::endl;
                ////DEBUG////std::cout << "src_pos.size()=" << src_pos.size() << std::endl;
                //if(this->ghdss_exec.ChangeSrcPos_TF_CONJ_W(this->src_pos, this->ghdss_frame_params) != 0){
                if(this->ghdss_exec.ChangeSrcPos_TF_CONJ_W(this->src_pos) != 0){
                    ////DEBUG////std::cout << "executed ChangeSrcPos_TF_CONJ_W() , return !0;" << std::endl;
                    this->ghdss_exec.Initialize_GHDSS(input_frames.rows(), this->src_pos.size(), 0, input_frames.cols());
                    ////DEBUG////std::cout << "executed Initialize_GHDSS()" << std::endl;
                }
            }
            // The original code has this implementation, but it doesn't make sense.
            //if(OutputSpec.bins.rows() != this->src_pos.size() ||
            //   OutputSpec.bins.cols() != input_frames.cols()){
            //    this->OutputSpec.resize(this->src_pos.size(), 0, input_frames.cols());
            //}
            this->OutputSpec.resize(this->src_pos.size(), 0, input_frames.cols());
            this->ghdss_exec.ExecuteGHDSS(this->InputSpec, this->OutputSpec);
            this->ghdss_exec.SetUpdateW();
            //this->ghdss_exec.AddUpdateW(this->ghdss_frame_params, this->exportw_filename, false);
            this->ghdss_exec.AddUpdateW(this->exportw_filename, false);
            for(size_t i = 0; i < this->OutputSpec.bins.rows(); i++){
                data out_vec = data::create<std::vector<std::complex<float> > >();
                auto &vec = out_vec.cast<std::vector<std::complex<float> > >();
                vec.resize(input_frames.cols());
                for(size_t j = 0; j < input_frames.cols(); j++){
                    vec[j] = this->OutputSpec.bins(i, j);
                }
                sep.insert(std::make_pair(this->src_pos[i].idpos, out_vec));
            }
        }

        //w.vsize.resize(4); // Tensor<float>
        w.vsize.resize(3); // Tensor<std::complex<float> >
        w.vsize[0] = OutputSpec.num_ch;
        w.vsize[1] = input_frames.rows(); // channels
        w.vsize[2] = input_frames.cols(); // frequency
        //w.vsize[3] = 2; // real, imag
        w.data.resize(w.vsize[0] * w.vsize[1] * w.vsize[2]);
        for(size_t kx = 0; kx < w.vsize[0]; kx++){
            for(size_t ky = 0; ky < w.vsize[1]; ky++){
                for(size_t kz = 0; kz < w.vsize[2]; kz++){
                    ////DEBUG////std::cout << "kx=" << kx << ", ky=" << ky << ", kz=" << kz << std::endl;
                    w.data[kx * w.vsize[2] * w.vsize[1] + ky * w.vsize[2] + kz] =
                        this->ghdss_exec.GetUpdateW(kx, ky, kz);
                }
            }
        }

        output["OUTPUT"].push(out_sep);
        output["OUTPUTW"].push(out_w);

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

        return true;
    }
}
}

/* end of file */
