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

#include <cmath>
#include <algorithm>
#include "hark-lib/node/LocalizationMUSIC.hpp"
#include "hark-lib/node/LocalizeMUSIC.h"

namespace hark
{
namespace node
{
    LocalizeMUSIC::LocalizeMUSIC():
        NodeDef("LocalizeMUSIC")
    {
        *this
            << Terminal::in("INPUT")
            << Terminal::in("NOISECM")
            << Terminal::in("TRANSFER_FUNCTION")
            << Terminal::in("OPERATION_FLAG")
            << Terminal::in("MUSIC_ALGORITHM").def("SEVD")
            << Terminal::in("TF_CHANNEL_SELECTION")
            << Terminal::in("LENGTH").def(512)
            << Terminal::in("SAMPLING_RATE").def(16000)
            << Terminal::in("TF_INPUT_TYPE").def("FILE")
            << Terminal::in("A_MATRIX").def("")
            << Terminal::in("WINDOW").def(50)
            << Terminal::in("WINDOW_TYPE").def("FUTURE")
            << Terminal::in("PERIOD").def(50)
            << Terminal::in("NUM_SOURCE").def(2)
            << Terminal::in("MIN_DEG").def(-180)
            << Terminal::in("MAX_DEG").def(180)
            << Terminal::in("LOWER_BOUND_FREQUENCY").def(500)
            << Terminal::in("UPPER_BOUND_FREQUENCY").def(2800)
            << Terminal::in("SPECTRUM_WEIGHT_TYPE").def("Uniform")
            << Terminal::in("A_CHAR_SCALING").def(1.0)
            << Terminal::in("MANUAL_WEIGHT_SPLINE")
            << Terminal::in("MANUAL_WEIGHT_SQUARE")
            << Terminal::in("ENABLE_EIGENVALUE_WEIGHT").def(true)
            //<< Terminal::in("ENABLE_INTERPOLATION").def(false)
            //<< Terminal::in("INTERPOLATION_TYPE").def("FTDLI")
            //<< Terminal::in("HEIGHT_RESOLUTION").def(1.0)
            //<< Terminal::in("AZIMUTH_RESOLUTION").def(1.0)
            //<< Terminal::in("RANGE_RESOLUTION").def(1.0)
            << Terminal::in("PEAK_SEARCH_ALGORITHM").def("LOCAL_MAXIMUM")
            << Terminal::in("MAXIMUM_OUT_PEAKS").def(-1)
            << Terminal::in("DEBUG").def(false)
            >> Terminal::out("OUTPUT")
            >> Terminal::out("SPECTRUM")
            >> Terminal::out("SPECTRUM_BIN")
            >> Terminal::out("TF_INFO")
            >> Terminal::out("RXXS")
            >> Terminal::out("RXXN");
    }

    void
    LocalizeMUSIC::reset()
    {
    }

    void
    LocalizeMUSIC::initialize(InputDataBundle &input)
    {
        this->music_algorithm = input["MUSIC_ALGORITHM"].last().cast<std::string>();
        this->channel_selection = input["TF_CHANNEL_SELECTION"].last().as<std::vector<int> >();
        this->length = input["LENGTH"].last().as<int>();
        this->samplingRate = input["SAMPLING_RATE"].last().as<int>();
        this->select_tf_type = input["TF_INPUT_TYPE"].last().cast<std::string>();
        this->FilenameAMatrix = input["A_MATRIX"].last().cast<std::string>();
        this->window = input["WINDOW"].last().as<int>();
        this->window_type = input["WINDOW_TYPE"].last().cast<std::string>();
        this->period = input["PERIOD"].last().as<int>();
        this->num_source = input["NUM_SOURCE"].last().as<int>();
        this->mindeg = input["MIN_DEG"].last().as<int>();
        this->maxdeg = input["MAX_DEG"].last().as<int>();
        this->lower_bound = input["LOWER_BOUND_FREQUENCY"].last().as<int>();
        this->upper_bound = input["UPPER_BOUND_FREQUENCY"].last().as<int>();
        this->spectrum_weight_type = input["SPECTRUM_WEIGHT_TYPE"].last().cast<std::string>();
        this->t_s = input["A_CHAR_SCALING"].last().as<float>();
        ////this->SSW = input["MANUAL_WEIGHT_SPLINE"].last().cast<Matrix<float> >();
        ////this->SSW_SQ = input["MANUAL_WEIGHT_SQUARE"].last().cast<std::vector<float> >();
        this->enable_eigenvalue_weight = input["ENABLE_EIGENVALUE_WEIGHT"].last().as<bool>();
        //this-> = input["ENABLE_INTERPOLATION"].last().as<bool>();
        //this-> = input["INTERPOLATION_TYPE"].last().cast<std::string>();
        //this-> = input["HEIGHT_RESOLUTION"].last().as<float>();
        //this-> = input["AZIMUTH_RESOLUTION"].last().as<float>();
        //this-> = input["RANGE_RESOLUTION"].last().as<float>();
        //this-> = input["PEAK_SEARCH_ALGORITHM"].last().cast<std::string>();
        this->maxnum_out_peaks = input["MAXIMUM_OUT_PEAKS"].last().as<int>();
        this->is_debug = input["DEBUG"].last().as<bool>();

        this->pslength = this->length / 2 + 1;

        if(this->spectrum_weight_type == "Manual_Square"){
            std::vector<float> SSW_SQ;
            SSW_SQ = input["MANUAL_WEIGHT_SQUARE"].last().cast<std::vector<float> >();
            this->MSSW.resize(this->pslength);
            for(size_t i = 0; i < this->pslength; i++){
                int SSW_j = -1;
                for(size_t j = 0; j < SSW_SQ.size(); j++){
                    if(SSW_SQ[j] <= this->samplingRate * (float) i / (float) this->length){
                        SSW_j = j;
                    }
                }
                if(SSW_j % 2 == 0){
                    this->MSSW[i] = 1.0;
                }
                else{
                    this->MSSW[i] = 0.0;
                }
            }
        }
        else{
            Matrix<float> SSW;
            if(this->spectrum_weight_type == "Manual_Spline"){ 
                SSW = input["MANUAL_WEIGHT_SPLINE"].last().cast<Matrix<float> >();
            }
            else if(this->spectrum_weight_type == "Uniform"){
                SSW.resize(2, 5);
                SSW(0, 0) = 0.0f / 4.0f / 2.0f * (float) this->samplingRate;
                SSW(0, 1) = 1.0f / 4.0f / 2.0f * (float) this->samplingRate;
                SSW(0, 2) = 2.0f / 4.0f / 2.0f * (float) this->samplingRate;
                SSW(0, 3) = 3.0f / 4.0f / 2.0f * (float) this->samplingRate;
                SSW(0, 4) = 4.0f / 4.0f / 2.0f * (float) this->samplingRate;
                SSW(1, 0) = 1.0f;
                SSW(1, 1) = 1.0f;
                SSW(1, 2) = 1.0f;
                SSW(1, 3) = 1.0f;
                SSW(1, 4) = 1.0f;
            }
            else if(this->spectrum_weight_type == "A_Characteristic"){
                SSW.resize(2, 16);
                SSW(0, 0)  = 10.0f * this->t_s;                SSW(0, 1)  = 12.5f * this->t_s;
                SSW(0, 2)  = 16.0f * this->t_s;                SSW(0, 3)  = 20.0f * this->t_s;
                SSW(0, 4)  = 31.5f * this->t_s;                SSW(0, 5)  = 63.0f * this->t_s;
                SSW(0, 6)  = 125.0f * this->t_s;               SSW(0, 7)  = 250.0f * this->t_s;
                SSW(0, 8)  = 500.0f * this->t_s;               SSW(0, 9)  = 1000.0f * this->t_s;
                SSW(0, 10) = 2000.0f * this->t_s;              SSW(0, 11) = 4000.0f * this->t_s;
                SSW(0, 12) = 8000.0f * this->t_s;              SSW(0, 13) = 12500.0f * this->t_s;
                SSW(0, 14) = 16000.0f * this->t_s;             SSW(0, 15) = 20000.0f * this->t_s;
                SSW(1, 0)  = std::pow(10.0f, -70.4f / 20.0f);  SSW(1, 1)  = std::pow(10.0f, -63.4f / 20.0f);
                SSW(1, 2)  = std::pow(10.0f, -56.7f / 20.0f);  SSW(1, 3)  = std::pow(10.0f, -50.5f / 20.0f);
                SSW(1, 4)  = std::pow(10.0f, -39.4f / 20.0f);  SSW(1, 5)  = std::pow(10.0f, -26.2f / 20.0f);
                SSW(1, 6)  = std::pow(10.0f, -16.1f / 20.0f);  SSW(1, 7)  = std::pow(10.0f, -8.6f / 20.0f);
                SSW(1, 8)  = std::pow(10.0f, -3.2f / 20.0f);   SSW(1, 9)  = std::pow(10.0f, 0.0f / 20.0f);
                SSW(1, 10) = std::pow(10.0f, 1.2f / 20.0f);    SSW(1, 11) = std::pow(10.0f, 1.0f / 20.0f);
                SSW(1, 12) = std::pow(10.0f, -1.1f / 20.0f);   SSW(1, 13) = std::pow(10.0f, -4.3f / 20.0f);
                SSW(1, 14) = std::pow(10.0f, -6.6f / 20.0f);   SSW(1, 15) = std::pow(10.0f, -9.3f / 20.0f);
            }
            std::vector<float> KEY_X(SSW.cols());
            std::vector<float> KEY_Y(SSW.cols());
            for(size_t i = 0; i < SSW.cols(); i++){
                KEY_X[i] = SSW(0, i);
                KEY_Y[i] = SSW(1, i);
            }
            std::vector<float> SPLINE_X(this->pslength);
            std::vector<float> SPLINE_Y(this->pslength);
            for(size_t i = 0; i < SPLINE_X.size(); i++){
                SPLINE_X[i] = this->samplingRate * (float)i / (float) this->length;
                if((SPLINE_X[i] < SSW(0, 0)) || (SPLINE_X[i] > SSW(0, SSW.cols() - 1))){
                    SPLINE_Y[i] = 0.0;
                    continue;
                }
                SPLINE_Y[i] = cubic_spline(KEY_X, KEY_Y, SPLINE_X[i]);
            }
            //this->MSSW.resize(this->pslength);
            //this->MSSW = SPLINE_Y;
            // noalias(this->MSSW) = SPLINE_Y / (float)ublas::sum(SPLINE_Y) * (float) this->pslength;
            //noalias(this->MSSW) = SPLINE_Y;
            this->MSSW.swap(SPLINE_Y);
        }

        if(this->window_type == "FUTURE"){
            look_ahead = this->window - 1;
            look_back = 0;
        }
        else if(this->window_type == "MIDDLE"){
            look_ahead = this->window / 2;
            look_back = this->window / 2 + (this->window % 2) - 1;
        }
        else if(this->window_type == "PAST"){
            look_ahead = 0;
            look_back  = 0;
        }

        //inputsCache[inputID].lookAhead = look_ahead;
        //inputsCache[inputID].lookBack = look_back;

        sum_count = 0;

        this->bInit_flag = false;
    }

    bool
    LocalizeMUSIC::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
        //const auto &input1 = input["INPUT"].last().cast<Matrix<std::complex<float> > >();
        //const auto &noise = input["NOISECM"].last().cast<Matrix<std::complex<float> > >();
        const auto &tfname = input["TFNAME"].last().cast<std::string>();
        const auto &inputtf = input["TRANSFER_FUNCTION"].last().cast<TransferFunction>();
        const auto &operation_flag = input["OPERATION_FLAG"].last().as<bool>();

        std::vector<data> tf_info;


        //cout << "count = " << count << endl;
        TransferFunction tfData;
        if(this->select_tf_type == "ONLINE") {
            if(inputtf.IsValid()) {
                this->tfData = inputtf;
            }
            else {
                std::stringstream ss;
                ss << "LocalizeMUSIC: TRANSFER_FUNCTION input is not set. Add input 'TRANSFER_FUNCTION'\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                throw RuntimeError(ss.str());
            }
        }
        else if(this->select_tf_type == "FILE") {
            if(! this->tfData.IsValid()) {
                if(inputtf.IsValid()) {
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: The TF_INPUT_TYPE parameter has been set to FILE. If you set a valid transfer function on the TRANSFER_FUNCTION input, it will be ignored.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    std::cerr << ss.str() << std::endl;
                }
            }
        }
        else {
            std::stringstream ss;
            ss << "LocalizeMUSIC: \n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }

        if(bInit_flag == false) {// Initialize
            if(this->channel_selection.size() != 0) {
                this->nb_channels = (int) this->channel_selection.size();
                this->ch_selection.resize(this->nb_channels);
                for(size_t i = 0; i < this->nb_channels; i++) {
                    this->ch_selection[i] = this->channel_selection[i];
                }
            }
            else {
                if(this->select_tf_type == "ONLINE") {
                    this->nb_channels = tfData.GetMicrophones().size();
                }
                else if(this->select_tf_type == "FILE") {
                    this->nb_channels = localizer->GetTFNumChannels(const_cast<char *>(this->FilenameAMatrix.c_str()));
                }
                if(this->nb_channels < 0) {
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: The number of channels in the transfer function file \"" << this->FilenameAMatrix << "\" is negative. (channels=" << this->nb_channels << ")\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                ch_selection.resize(this->nb_channels);
                for(size_t i = 0; i < this->nb_channels; i++) {
                    ch_selection[i] = i;
                }
            }

            Matrix<std::complex<float> > x(this->nb_channels, this->pslength);

            xbuf.resize(0);
            this->Rxx.resize(this->pslength);
            this->RxxN.resize(this->pslength);
            for(int k = 0; k < this->pslength; k++) {
                Matrix<std::complex<float> > tmp_rxxs(this->nb_channels, this->nb_channels);
                Matrix<std::complex<float> > tmp_rxxn(this->nb_channels, this->nb_channels);
                this->Rxx[k] = tmp_rxxs;
                this->RxxN[k] = tmp_rxxn;

                //for(size_t i = 0; i < this->nb_channels; i++) {
                //    for(size_t j = 0; j < this->nb_channels; j++) {
                //        if(i==j){
                //            this->RxxN[k](i, j) = std::complex<float>(1.0f, 0.0f);
                //        }
                //        else{
                //            this->RxxN[k](i, j) = std::complex<float>(0.0f, 0.0f);
                //        }
                //    }
                //}
                this->RxxN[k].setIdentity();
            }

            if(this->num_source >= this->nb_channels) {
                std::stringstream ss;
                ss << "LocalizeMUSIC: Number of sources should be less than number of channels. (sources=" << this->num_source << ", channels=" << this->nb_channels << ")\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                throw RuntimeError(ss.str());
            }
            localizer.reset(new LocalizationMUSIC(this->nb_channels, this->samplingRate,
                            this->length, this->length, this->maxnum_out_peaks));

            if(this->select_tf_type == "FILE") {
                std::cout << "reading A matrix\n";
                if(!localizer->ReadTransferFunction(const_cast<char *>(this->FilenameAMatrix.c_str()), ch_selection)) {
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: An exception was caught when reading the transfer function file \"" << this->FilenameAMatrix << "\" set to A_MATRIX.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                std::cout << "done\n";
            }
            else if(this->select_tf_type == "ONLINE") {
                const std::string tfwhat = tfData.GetWhatIsThis();
                if(tfwhat.compare("partial transfer function") == 0) {
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: TRANSFER_FUNCTION is not initialized.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    throw RuntimeError(ss.str());
                    //localizer->SetTransferFunctionPertial(tfData, ch_selection);
                }
                else if(tfwhat.compare("transfer function") == 0) {
                    localizer->SetTransferFunctionAll(tfData, ch_selection);
                }
            }

            localizer->SetNumSource(this->num_source);
            localizer->SetUsedFrequency(this->lower_bound, this->upper_bound);
            ptfname = this->FilenameAMatrix;

            this->nid = localizer->GetSearchId().size();

            //tf_info = std::vector<Source>();
            for(size_t i = 0; i < localizer->GetSearchId().size(); i++) {
                Source src;
                src.id = i;
                src.power = 0.0f;
                src.x[0] = localizer->GetSearchId()[i][0];
                src.x[1] = localizer->GetSearchId()[i][1];
                src.x[2] = localizer->GetSearchId()[i][2];
                tf_info.push_back(data(src));
            }
            bInit_flag = true;
        }
        else if(this->select_tf_type == "ONLINE") { // not Initialize
            const std::string  tfwhat = tfData.GetWhatIsThis();
            if(tfwhat.compare("partial transfer function") == 0) {
                localizer->SetTransferFunctionPertial(tfData, ch_selection);
            }
            else if(tfwhat.compare("transfer function") == 0) {
                localizer->SetTransferFunctionAll(tfData, ch_selection);
            }
        }

        //if((paramsID != -1)&&(parammapID == -1)){
        //    RCPtr<Vector<ObjectRef> > param_ptr = getInput(paramsID, count);
        //    const Vector<ObjectRef>& param_vec = *param_ptr;      
        //    if(param_vec.size() != 0){        
        //        RCPtr<ParamsLocalizeMUSIC> value_LocalizeMUSIC = param_vec[0];
        //        this->num_source = (*value_LocalizeMUSIC).num_source;
        //        localizer->SetNumSource(this->num_source);
        //        this->mindeg = (*value_LocalizeMUSIC).min_deg;
        //        this->maxdeg = (*value_LocalizeMUSIC).max_deg;
        //        this->lower_bound = (*value_LocalizeMUSIC).lower_bound_frequency;
        //        this->upper_bound = (*value_LocalizeMUSIC).upper_bound_frequency;
        //        localizer->SetUsedFrequency(this->lower_bound, this->upper_bound);
        //    }
        //}

        //const auto &tfname = input["TFNAME"].last().cast<std::string>();
        if(!tfname.empty()){
            if(ptfname.compare(tfname) != 0){
                if(!localizer->ReadTransferFunction(const_cast<char *>(tfname.c_str()), ch_selection)){
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: An exception was caught when reading the transfer function file \"" << tfname << "\" set to TFNAME.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    throw RuntimeError(ss.str());
                }
                this->nid = localizer->GetSearchId().size();
            }
            ptfname = tfname;
        }

//////////////////////////////
        Matrix<std::complex<float> > x(this->nb_channels, this->pslength);

        // Load Noise Correlation MatrixLocalizationMUSIC

        if(this->music_algorithm != "SEVD"){
            const auto &noise = input["NOISECM"].last().cast<Matrix<std::complex<float> > >();
            //noise.rows() is number of elements of frequency series (should be equal to pslength)

            // Sets the noise correlation matrix if the entered NOISECM is not empty.
            if(noise.size() != 0) {
                for(size_t k = 0; k < noise.rows(); k++) {
                    for(size_t i = 0; i < this->nb_channels; i++) {
                        for(size_t j = 0; j < this->nb_channels; j++) {
                            this->RxxN[k](i, j) = noise(k, i * this->nb_channels + j);
                        }
                    }
                }
            }
        }

        // Save data vector for spatial spectrum
        auto out_spectrum = data::create<std::vector<float> >(this->nid);
        auto &spectrum = out_spectrum.cast<std::vector<float> >();
        //for(size_t i = 0; i < this->nid; i++) {
        //    spectrum[i] = 0.0f;
        //}
        std::fill(spectrum.begin(), spectrum.end(), 0.0f);

        // Save data vector for spatial spectrum
        auto out_spectrumbin = data::create<Tensor<float> >();
        auto &spectrumbin = out_spectrumbin.cast<Tensor<float> >();
        spectrumbin.vsize.resize(2);
        spectrumbin.vsize[0] = this->pslength;
        spectrumbin.vsize[1] = this->nid;
        spectrumbin.data.resize(this->pslength * this->nid);
        //for(size_t i = 0; i < spectrumbin.data.size(); i++) {
        //    spectrumbin.data[i] = 0.0f;
        //}
        std::fill(spectrumbin.data.begin(), spectrumbin.data.end(), 0.0f);

        //auto out_tf_info = data::create<std::vector<Source> >();
        //auto &tf_info = out_tf_info.cast<std::vector<Source> >();
        //std::vector<data> tf_info;

        auto out_rxxs = data::create<Matrix<std::complex<float> > >(this->Rxx.size(), this->Rxx[0].rows() * this->Rxx[0].cols());
        auto &rxxs = out_rxxs.cast<Matrix<std::complex<float> > >();
        //for(size_t k = 0; k < Rxx.size(); k++) {
        //    for(size_t i = 0; i < Rxx[k].rows() ; i++) {
        //        for(size_t j = 0; j < Rxx[k].cols(); j++) {
        //            rxxs(k, i*Rxx[k].cols() + j) = 0.0f;
        //        }
        //    }
        //}
        rxxs.setZero();

        auto out_rxxn = data::create<Matrix<std::complex<float> > >(this->RxxN.size(), this->RxxN[0].rows() * this->RxxN[0].cols());
        auto &rxxn = out_rxxn.cast<Matrix<std::complex<float> > >();
        //for(size_t k = 0; k < this->RxxN.size(); k++) {
        //    for(size_t i = 0; i < this->RxxN[k].rows(); i++) {
        //        for(size_t j = 0; j < this->RxxN[k].cols(); j++) {
        //            rxxn(k, i * this->RxxN[k].cols() + j) = 0.0f;
        //        }
        //    }
        //}   
        rxxn.setZero();

        // Current Correlation Matrix Generation

        // Vector<ObjectRef>& sources = *new Vector<ObjectRef>;
        // (*(outputs[outputID].buffer))[count] = &sources;
        //RCPtr<Vector<ObjectRef> > sources(Vector<ObjectRef>::alloc(0));
        //(*(outputs[outputID].buffer))[count] = sources;
        std::vector<data> sources;

        //const auto &operation_flag = input["OPERATION_FLAG"].last().as<bool>();
        if(!operation_flag){ return false; }

        if(this->window_type == "PAST"){
            // inputCashe 
            const auto &input1 = input["INPUT"].last().cast<Matrix<std::complex<float> > >();
            //inputp = getInput(inputID, count); 
            if(count == 0) { 
                if(ch_selection.size() != input1.rows()) {
                    std::stringstream ss;
                    ss << "LocalizeMUSIC: Input channel size is not matched to the steering array. (input=" << input1.rows() << ", expected=" << ch_selection.size() << ")\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                    throw RuntimeError(ss.str());
                }
            }
            for(int chan = 0; chan < this->nb_channels; chan++) { 
                for(int k = 0; k < this->pslength; k++) { 
                    x(chan, k) = input1(chan, k);
                }
            }
            if(xbuf.size() < this->window){
                xbuf.push_back(x);
            }
            else{
                xbuf.push_back(x);
                xbuf.pop_front();
            }
      
            if((count % this->period == 0) && ((count != 0) || (this->period == 1))) {
                for(size_t i = 0; i < xbuf.size(); i++) {
                    for(size_t chan = 0; chan < this->nb_channels; chan++) {
                        for(size_t k = 0; k < this->pslength; k++) {
                            x(chan, k) = xbuf[i](chan, k);
                        }
                    }
                    AddToCorrelation(Rxx, localizer->GetUsedFrequency(), x);
                    sum_count++;
                }
                NormalizeCorrelation(Rxx, localizer->GetUsedFrequency(), sum_count);
                sum_count = 0;
            }
            else {
                return false;
            }
        }
        else{
            if(count % this->period == 0 && count >= look_back) { 
                for(size_t i = count - look_back; i <= count + look_ahead; i++) { 
                    const auto &input1 = input["INPUT"].at(i).cast<Matrix<std::complex<float> > >();
                    //inputp = getInput(inputID, count + i); 
                    if(count == 0) { 
                        if(ch_selection.size() != input1.rows()) {
                            std::stringstream ss;
                            ss << "LocalizeMUSIC: Input channel size is not matched to the steering array. (input=" << input1.rows() << ", expected=" << ch_selection.size() << ")\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                            throw RuntimeError(ss.str());
                        }
                    }
                    for(size_t chan = 0; chan < this->nb_channels; chan++) {
                        for(size_t k = 0; k < this->pslength; k++) {
                            x(chan, k) = input1(chan, k);
                        }
                    }
                    AddToCorrelation(Rxx, localizer->GetUsedFrequency(), x);
                    sum_count++;
                }
                NormalizeCorrelation(Rxx, localizer->GetUsedFrequency(), sum_count);
                sum_count = 0;
            }
            else {
                return false;
            }
        }

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

        // MUSIC with GEVD

        localizer->Localize(this->Rxx, this->RxxN, this->mindeg, this->maxdeg, this->music_algorithm, this->MSSW, this->enable_eigenvalue_weight);

        int n_result = localizer->GetFoundSourceCount();

        for(size_t i = 0; i < n_result; i++) {
            const int found_id = localizer->GetFoundId()[i];
            if(this->is_debug) {
                printf("%ld: %7.2f %7.2f %7.2f %7.2f\t", i,
                (float) (localizer->GetSearchId()[found_id][0]),
                (float) (localizer->GetSearchId()[found_id][1]),
                (float) (localizer->GetSearchId()[found_id][2]),
                localizer->GetP(localizer->GetFoundId()[i]));
            }
            Source src;
            src.id = 0;
            src.power = localizer->GetP(found_id);
            src.x[0] = localizer->GetSearchId()[found_id][0];
            src.x[1] = localizer->GetSearchId()[found_id][1];
            src.x[2] = localizer->GetSearchId()[found_id][2];
            src.tfindex[0] = found_id;
            src.tfindex[1] = 0;
            src.tfindex[2] = 0;
            src.tfindex_max[0] = localizer->GetSearchId().size() - 1;
            src.tfindex_max[1] = 0;
            src.tfindex_max[2] = 0;
            src.tfindex_min[0] = 0;
            src.tfindex_min[1] = 0;
            src.tfindex_min[2] = 0;
            src.compare_mode = Source::TFINDEX; // 2015/02/25 added flag
            sources.push_back(data(src));
        }

        if(this->is_debug) {
            printf("\n");
            printf("MUSIC spectrum: ");
            for(size_t i = 0; i < this->nid; i++) {
                printf("%f ", localizer->GetP(i));
            }
            printf("\n");
        }

        // Output the spacial spectrum
        if(true){
            for(size_t i = 0; i < this->nid; i++) {
                spectrum[i] = localizer->GetP(i);
            }
        }

        // Output the spacial spectrum of each bin
        if(true){
            for(size_t bin = 0; bin < this->pslength; bin++) {
                for(size_t iid = 0; iid < this->nid; iid++) {
                    spectrumbin.data[iid + bin * this->nid] = localizer->GetP_bin(bin, iid);
                }
            }
        }

        if(true){
            for(size_t k = 0; k < this->Rxx.size(); k++) {
                for(size_t i = 0; i < this->Rxx[k].rows(); i++) {
                    for(size_t j = 0; j < this->Rxx[k].cols(); j++) {
                        rxxs(k, i * this->Rxx[k].cols() + j) = this->Rxx[k](i, j);
                    }
                }
            }
        }

        if(true){
            for(size_t k = 0; k < this->RxxN.size(); k++) {
                for(size_t i = 0; i < this->RxxN[k].rows(); i++) {
                    for(size_t j = 0; j < this->RxxN[k].cols(); j++) {
                        rxxn(k, i * this->RxxN[k].cols() + j) = this->RxxN[k](i, j);
                    }
                }
            }
        }

        ClearCorrelation(Rxx, localizer->GetUsedFrequency());
        ClearCorrelation(RxxN, localizer->GetUsedFrequency());

        //output["OUTPUT"].push(input["INPUT"].last());
        output["OUTPUT"].push(data(std::move(sources)));
        output["SPECTRUM"].push(out_spectrum);
        output["SPECTRUM_BIN"].push(out_spectrumbin);
        //output["TF_INFO"].push(out_tf_info);
        output["TF_INFO"].push(data(std::move(tf_info)));
        output["RXXS"].push(out_rxxs);
        output["RXXN"].push(out_rxxn);

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

        return true;
    }
}
}

/* end of file */
