
#define ENABLE_TIMER 0

#include <chrono>
#include <algorithm>

#if ENABLE_TIMER
#include "strDurlog.hpp"
#endif

#ifdef ENABLE_HARKIO_NH

#include "LocalizationMUSIC.hpp"

#include <iostream>
#include <fstream>
#include <math.h>

#ifdef ENABLE_HARKIO3
extern "C" {
#include <libharkio3.h>
}
#endif // #ifdef ENABLE_HARKIO3

#include <functional>
#include "matrix.h"

//#include "evd.hpp"
//#include "svd.hpp"
//#include "svdold.hpp" 

struct less_for_vec {
    bool operator()(std::vector<float>& v, int i, int j) {
        return v[i] < v[j];
    }
};

LocalizationMUSIC::LocalizationMUSIC(int num_mic, int sampling_rate, int tlength, int shift, int maxn_out_peaks) :
    NumMicrophones(num_mic), TimeLength(tlength), Shift(shift), SamplingRate(sampling_rate), R(num_mic, num_mic), V(
        num_mic, num_mic), RN(num_mic, num_mic), VN(num_mic, num_mic), TempCalc1(num_mic, num_mic), TempCalc2(num_mic,
        num_mic), TempCalc3(num_mic, num_mic), TempCalc4(num_mic, num_mic), TempCalc5(num_mic, num_mic), TempCalc6(
        num_mic, num_mic), TempCalc7(num_mic, num_mic), TempCalc8(num_mic, num_mic), TempCalc9(num_mic, num_mic), NumSource(
        num_mic) {
    FreqLength = TimeLength / 2 + 1;

    UsesFreq.resize(FreqLength);

    maxnum_output_peaks = maxn_out_peaks;

    EigenValue.resize(FreqLength);
    CEigenValue.resize(NumMicrophones);
    REigenValue.resize(NumMicrophones);
    MaxRxx.resize(FreqLength);
    MaxRxxN.resize(FreqLength);
    E.resize(FreqLength);
    EN.resize(FreqLength);

#ifdef ENABLE_LAPACK
    for(size_t k = 0; k < FreqLength; k++) {
        EigenValue[k].resize(NumMicrophones);
        Matrix<std::complex<float> > Emat(NumMicrophones, NumMicrophones);
        Matrix<std::complex<float> > ENmat(NumMicrophones, NumMicrophones);
        E[k] = Emat;
        EN[k] = ENmat;
    }
#else
    for(size_t k = 0; k < FreqLength; k++) {
        EigenValue[k].resize(NumMicrophones);
        E[k].resize(NumMicrophones, NumMicrophones);
        EN[k].resize(NumMicrophones, NumMicrophones);
    }
#endif

    FoundId.resize(NumSource);
}

void LocalizationMUSIC::SetUsedFrequency(int lo, int up) {
    size_t lower = (int) ((float) lo * TimeLength / SamplingRate);
    size_t upper = (int) ((float) up * TimeLength / SamplingRate);
    for(size_t k = lower; k < upper; k++) {
        UsesFreq[k] = true;
    }
}

void LocalizationMUSIC::Localize(std::vector<Matrix<std::complex<float> > >& Rxx, std::vector<Matrix<std::complex<float> > >& RxxN,
                                 int mindeg, int maxdeg,
                                 std::string music_alg, std::vector<float>& MSSW, bool enable_eigenvalue_weight) {
#if ENABLE_TIMER
    std::chrono::system_clock::time_point start_time;
    std::chrono::system_clock::time_point end_time;
    start_time = std::chrono::system_clock::now();
#endif

    for(size_t k = 0; k < UsesFreq.size(); k++) {

        MaxRxx[k] = 1.0;
        MaxRxxN[k] = 1.0;

        if(!UsesFreq[k]) {
            continue;
        }
        float max = MaxOfAbsValue(Rxx[k]);
        R = Rxx[k] / (max + 1.0f);
        MaxRxx[k] = max + 1.0f;

        if(music_alg != "SEVD") {
            float maxN = MaxOfAbsValue(RxxN[k]);
            RN = RxxN[k] / (maxN);
            MaxRxxN[k] = maxN;
        }
        //============= GEVD ==============
        //printf("%d ", k);

        if(music_alg == "SEVD") {
#ifdef ENABLE_LAPACK
            Matrix<std::complex<float> > EV(NumMicrophones,1);
            R.eigenValues(EV,V);
            for(size_t ii = 0; ii < NumMicrophones; ii++){
                EigenValue[k][ii] = EV(ii, 0).real();
            }
            sort_eig_fvec_ascending(EigenValue[k], V, EigenValue[k], V);
            //sort_eig_fvec_descending(EigenValue[k], V, EigenValue[k], V);
#else // #ifdef ENABLE_LAPACK
            evd(R, EigenValue[k], V);
#endif // ENABLE_LAPACK
        }
        else if (music_alg == "GEVD") {
#ifdef ENABLE_LAPACK
            //RN.dump(stdout, "RN: %p"); ////////
            TempCalc1 = RN.Inverse();
            //TempCalc1.dump(stdout, "Tempcalc1: %p"); ////////
            Matrix<std::complex<float> > EV(NumMicrophones,1);
            //EV.dump(stdout, "EV: %p"); ////////
            TempCalc1.eigenValues(EV,VN);
            //TempCalc1.dump(stdout, "Tempcalc1(2): %p"); ////////
            //EV.dump(stdout, "EV(2): %p"); ////////
            //VN.dump(stdout, "VN: %p"); ////////
            for(int ii = 0; ii < NumMicrophones; ii++){
                REigenValue[ii] = EV(ii, 0).real();
            }
            sort_eig_fvec_ascending(REigenValue, VN, REigenValue, VN);//
            //sort_eig_fvec_descending(REigenValue, VN, REigenValue, VN);//
            //evd.hでは昇順で並べ替えられている
            //VN.dump(stdout, "VN(2): %p"); ////////
#else // #ifdef ENABLE_LAPACK
            invMAT(RN, TempCalc1);
            evd(TempCalc1, REigenValue, VN);
#endif // #ifdef ENABLE_LAPACK
            for(size_t i = 0; i < REigenValue.size(); i++) {
                if(REigenValue[i] < 0) {
                    REigenValue[i] = -1.0f * pow(-1.0f * REigenValue[i], (float) 0.5);
                }
                else {
                    REigenValue[i] = pow(REigenValue[i], (float) 0.5);
                }
            }

#ifdef ENABLE_LAPACK
            TempCalc2 = VN.Inverse();
            //TempCalc2.dump(stdout, "TempCalc2: %p"); ////////
            for(size_t i = 0; i < TempCalc3.rows(); i++) {
                for(size_t j = 0; j < TempCalc3.cols(); j++) {
                    if(i == j) {
                        TempCalc3(i, j) = REigenValue[i];
                    }
                    else {
                        TempCalc3(i, j) = 0.0f;
                    }
                }
            }
            //TempCalc3.dump(stdout, "TempCalc3: %p"); ////////
            TempCalc6 = TempCalc3 * TempCalc2;
            //TempCalc6.dump(stdout, "TempCalc6: %p"); ////////
            TempCalc4 = VN * TempCalc6;
            //R.dump(stdout, "R: %p"); ////////
            //TempCalc4.dump(stdout, "TempCalc4: %p"); ////////
            TempCalc7 = R * TempCalc4;
            //TempCalc7.dump(stdout, "TempCalc7: %p"); ////////
            TempCalc5 = TempCalc4 * TempCalc7;
            //TempCalc5.dump(stdout, "TempCalc5: %p"); ////////

            Matrix<std::complex<float> > EVtemp(NumMicrophones,1);
            //EVtemp.dump(stdout, "EVtemp: %p"); ////////
            TempCalc5.eigenValues(EVtemp,V);
            //TempCalc5.dump(stdout, "TempCalc5(2): %p"); ////////
            //EVtemp.dump(stdout, "EVtemp(2): %p"); ////////
            //V.dump(stdout, "V: %p"); ////////
            for(size_t ii = 0; ii < NumMicrophones; ii++){
                EigenValue[k][ii] = EVtemp.getElementAt(ii,0).real();
            }
            sort_eig_fvec_ascending(EigenValue[k], V, EigenValue[k], V);//
            //sort_eig_fvec_descending(EigenValue[k], V, EigenValue[k], V);//
#else // #ifdef ENABLE_LAPACK
            invMAT(VN, TempCalc2);
            for(size_t i = 0; i < TempCalc3.size1(); i++) {
                for(size_t j = 0; j < TempCalc3.size2(); j++) {
                    TempCalc3(i, j) = 0.0;
                    if(i == j) {
                        TempCalc3(i, j) = REigenValue[i];
                    }
                }
            }
            noalias(TempCalc6) = prod(TempCalc3, TempCalc2);
            noalias(TempCalc4) = prod(VN, TempCalc6);
            noalias(TempCalc7) = prod(R, TempCalc4);
            noalias(TempCalc5) = prod(TempCalc4, TempCalc7);
            evd(TempCalc5, EigenValue[k], V);
#endif // #ifdef ENABLE_LAPACK
        }
        else if (music_alg == "GSVD") {
#ifdef ENABLE_LAPACK
            TempCalc1 = RN.Inverse();
            TempCalc2= TempCalc1 * R;
            //TempCalc2.singularValueDecomposition(TempCalc3,TempCalc5,TempCalc4);//u,s,v
            TempCalc2.singularValueDecomposition(TempCalc4,TempCalc5,TempCalc3);//u,s,v
            for(size_t ii=0; ii<NumMicrophones; ii++){
                REigenValue[ii] = TempCalc5.getElementAt(ii,0).real();
            }
            sort_eig_fvec_ascending(REigenValue, TempCalc4, EigenValue[k], V);
            //sort_eig_fvec_descending(REigenValue, TempCalc4, EigenValue[k], V);
#else // #ifdef ENABLE_LAPACK
            invMAT(RN, TempCalc1);
            noalias(TempCalc2) = prod(TempCalc1, R);
            svd(TempCalc2, REigenValue, TempCalc4, TempCalc3);//a,s,u,v
            sort_eig_fvec_ascending(REigenValue, TempCalc4, EigenValue[k], V);
#endif // #ifdef ENABLE_LAPACK
        }

        E[k] = V;
    }

    //printf("\n");

    CalcAveragePower(EigenValue, E, A, UsesFreq, NumSource, Pavg, Pavg_bin, MSSW, enable_eigenvalue_weight);
    for(size_t ih = 0; ih < SearchId.size(); ih++) {
        Pavg_sum[ih] = 10 * log10(Pavg[ih] + 1.0f);
    }
    if(maxnum_output_peaks < 0){
        maxnum_output_peaks = NumSource;
    }
    FoundSourceCount = SearchLocation(Pavg_sum, Pavg, maxnum_output_peaks, FoundId, mindeg, maxdeg);
#if ENABLE_TIMER
    end_time = std::chrono::system_clock::now();
    std::cout << DurationLog(end_time - start_time) << std::endl;
#endif

}

int LocalizationMUSIC::GetTFNumChannels(char* filename) {
    int nmic = -1;
#ifdef ENABLE_HARKIO3
    harkio_TransferFunction *tf = NULL;
    tf = harkio_TransferFunction_fromFile(filename, "transfer function");
    if(tf == NULL){
        std::cerr << "LocalizationMUSIC. Failed to read tf file." << std::endl;
        return -1;
    }
    if(tf->loctfs == NULL){
        std::cerr << "LocalizationMUSIC. No localization matrix in tf file." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return -1;
    }
    nmic = tf->mics->size;
    if(tf->loctfs[0]->rows != nmic){
        std::cerr << "LocalizationMUSIC. The number of mics does not match to the size of tf file." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return -1;
    }
    harkio_TransferFunction_delete(&tf);
#endif
    return nmic;
}


bool LocalizationMUSIC::SetTransferFunctionAll(TransferFunction tfData, std::vector<int>& ch) {

    // Get Configuration
    const hark::Config config = tfData.GetConfig();

    // Get TransferFunction
    const std::map<int, Matrix<std::complex<float>>> mpLoc = tfData.GetLocalization();
    //const HARK::Matrix<std::complex<float>>    &mat = mpLoc.second();


    int slength = config.nfft / 2 + 1;
    if(mpLoc.begin()->second.ncols() != slength) {
        std::cerr << "LocalizationMUSIC. The number of bins does not match to the size of tf input." << std::endl;
        std::cerr << "slength = " << slength <<"   ncols = "<< mpLoc.begin()->second.ncols() << std::endl;
        return false;
    }

    //// Check FFT Length
    //if (config.nfft != TimeLength) {
    //    std::cerr << "LocalizationMUSIC. The FFT Length does not match between params and tf input." << std::endl;
    //    return false;
    //}

    //// Check sampling rate
    //if (config.samplingRate != SamplingRate) {
    //    std::cerr << "LocalizationMUSIC. SamplingRate does not match between params and tf input." << std::endl;
    //    return false;
    //}
    
    // Get Sources
    const std::vector<Position>    Sources = tfData.GetSources();
    int nsrc = Sources.size();
    const Neighbor    NH = tfData.GetNeighbors();

    if(NH.ids.size() != nsrc) {
        std::cerr << "LocalizationMUSIC. The number of loctfs does not match to the size of nbrs." << std::endl;
        return false;
    }
    
    // Get Microphones
    const std::vector<Position> Mics = tfData.GetMicrophones();
    int nmic = Mics.size();
    if(mpLoc.begin()->second.nrows() != nmic) {
        std::cerr << "LocalizationMUSIC. The number of mics does not match to the size of tf input." << std::endl;
        return false;
    }

    int nmic_selected = ch.size();
    for(size_t c = 0; c < nmic_selected; c++) {
        if(!(ch[c] >= 0 && ch[c] < nmic)) {
            std::cerr << "LocalizationMUSIC. Nonexistent channel is selected." << std::endl;
            return false;
        }
    }

    A.resize(nsrc);
    for(size_t iid = 0; iid < nsrc; iid++) {
        Matrix<std::complex<float> > Amat(slength, nmic_selected);
        A[iid] = Amat;
    }

    for(size_t iid = 0; iid < nsrc; iid++) {
        const Matrix<std::complex<float> > &mat = mpLoc.at(iid);
        for(size_t m = 0; m < nmic_selected; m++) {
            for(size_t k = 0; k < slength; k++) {
                A[iid](k, m) = mat(ch[m], k);
            }
        }
    }

    // Source Info
    AId.resize(nsrc);
    for(size_t iid = 0; iid < nsrc; iid++) {
        AId[iid] = Sources[iid].id;
        //AId[iid] = tf->poses->pos[iid]->id;
    }

    SearchId.resize(nsrc);
    for(size_t iid = 0; iid < nsrc; iid++) {
        SearchId[iid].resize(3);
        if (Sources[iid].sys == Cartesian) {
            SearchId[iid][0] = Sources[iid].coord[0];
            SearchId[iid][1] = Sources[iid].coord[1];
            SearchId[iid][2] = Sources[iid].coord[2];
        }
        if(Sources[iid].sys == Polar) {
            float azimuth_radian = harkio_Position_convToRadian(Sources[iid].coord[0]);
            float elevation_radian = harkio_Position_convToRadian(Sources[iid].coord[1]);
            float coef = Sources[iid].coord[2] * cosf(elevation_radian);
            SearchId[iid][0] = coef * cosf(azimuth_radian);
            SearchId[iid][1] = coef * sinf(azimuth_radian);
            SearchId[iid][2] = Sources[iid].coord[2] * sinf(elevation_radian);
        }
    }

    nbrs_ids.resize(NH.ids.size());
    for(size_t i = 0; i < NH.ids.size(); i++) {
        nbrs_ids[i] = NH.ids[i];
    }
    nbrs_numNeighbors.resize(NH.neighbors.size());
    for(size_t i = 0; i < NH.neighbors.size(); i++) {
        nbrs_numNeighbors[i] = NH.neighbors[i].size();
    }

    
    nbrs_neighbors.resize(NH.ids.size());
    for(size_t i = 0; i < NH.ids.size(); i++) {
        nbrs_neighbors[i].resize(nbrs_numNeighbors[i]);
        for(size_t j = 0; j < nbrs_numNeighbors[i]; j++) {
            nbrs_neighbors[i][j] = NH.neighbors[i][j];
        }
    }

    Pavg_sum.resize(SearchId.size());
    Pavg.resize(SearchId.size());


    Pavg_bin.resize(A[0].rows());
    for(size_t bins = 0; bins < A[0].rows(); bins++) {
        Pavg_bin[bins].resize(SearchId.size());
    }

    std::cerr << SearchId.size() << " srcs, " << A[0].cols() << " microphones, " << (A[0].rows() - 1) * 2 << " points" << std::endl;

    return true;
}

bool LocalizationMUSIC::SetTransferFunctionPertial(TransferFunction tfData, std::vector<int>& ch) {

    if(A.size() <= 0) {
        std::cerr << "LocalizationMUSIC. TransferFunction must be inputed to set the Pertial TransferFunction." << std::endl;
    }

    // Get TransferFunction
    const std::map<int, Matrix<std::complex<float> > > mpLoc = tfData.GetLocalization();

    // Check Bin size 
    size_t slength = A[0].rows();
    if(mpLoc.begin()->second.cols() != slength) {
        std::cerr << "LocalizationMUSIC. The number of bins does not match to the size of tf input (Pertial)." << std::endl;
        std::cerr << "slength = " << slength << "   ncols = " << mpLoc.begin()->second.cols() << std::endl;
        return false;
    }

    // Check Num of Microphones
    size_t nmic = A[0].cols();
    if(mpLoc.begin()->second.rows() != nmic) {
        std::cerr << "LocalizationMUSIC. The number of mics does not match to the size of tf input." << std::endl;
        return false;
    }

    size_t nmic_selected = ch.size();
    for(size_t c = 0; c < nmic_selected; c++) {
        if (!(ch[c] >= 0 && ch[c] < nmic)) {
            std::cerr << "LocalizationMUSIC. Nonexistent channel is selected." << std::endl;
            return false;
        }
    }

    for(auto itrm = mpLoc.begin(); itrm != mpLoc.end(); ++itrm) {
        // Find the index of A by using srcID
        auto itrv = std::find(AId.begin(), AId.end(), itrm->first);
        if(itrv == AId.end()) continue; // not find
        const int src_ind = std::distance(AId.begin(), itrv);
        for(size_t m = 0; m < nmic_selected; m++) {
            for(size_t k = 0; k < slength; k++) {
                A[src_ind](k, m) = itrm->second(ch[m], k);
            }
        }
    }
    return true;
}

bool LocalizationMUSIC::ReadTransferFunction(char* filename, std::vector<int>& ch) {

#ifdef ENABLE_HARKIO3
    harkio_TransferFunction *tf = NULL;
    tf = harkio_TransferFunction_fromFile(filename, "transfer function");
    // tf = harkio_TransferFunction_new();
    // libharkio3_TransferFunction_readlibhakio2(&tf, filename, NULL);
    // ck_assert(tf != NULL);
    if(tf == NULL) {
        std::cerr << "LocalizationMUSIC. Failed to read tf file." << std::endl;
        return false;
    }

    if(tf->loctfs == NULL) {
        std::cerr << "LocalizationMUSIC. No localization matrix in tf file." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return false;
    }
    int slength = tf->cfg->nfft / 2 + 1;
    if(tf->loctfs[0]->cols != slength) {
        std::cerr << "LocalizationMUSIC. The number of bins does not match to the size of tf file." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return false;
    }

    int nsrc = tf->poses->size;
    // if ( tf->loctfs.size() != nsrc) {
    //   std::cerr << "LocalizationMUSIC. The number of loctfs does not match to the number of srcs." << std::endl;
    //   harkio_TransferFunction_delete(&tf);
    //   return false;
    // }
    if(tf->nbrs->size != nsrc) {
        std::cerr << "LocalizationMUSIC. The number of loctfs does not match to the size of nbrs." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return false;
    }

    int nmic = tf->mics->size;
        if(tf->loctfs[0]->rows != nmic) {
        std::cerr << "LocalizationMUSIC. The number of mics does not match to the size of tf file." << std::endl;
        harkio_TransferFunction_delete(&tf);
        return false;
    }

    int nmic2 = ch.size();
    for(size_t c = 0; c < ch.size(); c++) {
        if(!(ch[c] >= 0 && ch[c] < nmic)) {
            std::cerr << "LocalizationMUSIC. Nonexistent channel is selected." << std::endl;
            harkio_TransferFunction_delete(&tf);
           return false;
        }
    }
    A.resize(nsrc);

    for(size_t iid = 0; iid < nsrc; iid++) {
        Matrix<std::complex<float> > Amat(slength, nmic2);
        A[iid] = Amat;
    }
    COMPLEX_TYPE complexValue;
    for(size_t iid = 0; iid < nsrc; iid++) {
        for(size_t m = 0; m < nmic2; m++) {
            for(size_t k = 0; k < slength; k++) {
                harkio_Matrix_getValueComplex(tf->loctfs[iid], ch[m], k, &complexValue);
                A[iid](k, m) = complexValue;
            }
        }
    }

    AId.resize(nsrc);
    for(size_t iid = 0; iid < nsrc; iid++) {
        AId[iid] = tf->poses->pos[iid]->id;
    }

    SearchId.resize(nsrc);
    for(size_t iid = 0; iid < nsrc; iid++) {
        SearchId[iid].resize(3);
        if(tf->poses->pos[iid]->sys == Cartesian) {
            SearchId[iid][0] = tf->poses->pos[iid]->coord[0];
            SearchId[iid][1] = tf->poses->pos[iid]->coord[1];
            SearchId[iid][2] = tf->poses->pos[iid]->coord[2];
        }
        if(tf->poses->pos[iid]->sys == Polar) {
            float azimuth_radian = harkio_Position_convToRadian(tf->poses->pos[iid]->coord[0]);
            float elevation_radian = harkio_Position_convToRadian(tf->poses->pos[iid]->coord[1]);
            float coef = tf->poses->pos[iid]->coord[2] * cosf(elevation_radian);
            SearchId[iid][0] = coef * cosf(azimuth_radian);
            SearchId[iid][1] = coef * sinf(azimuth_radian);
            SearchId[iid][2] = tf->poses->pos[iid]->coord[2] * sinf(elevation_radian);
        }
    }

    nbrs_ids.resize(tf->nbrs->size);
    for(size_t i = 0; i < tf->nbrs->size; i++) {
        nbrs_ids[i] = tf->nbrs->ids[i];
    }
    nbrs_numNeighbors.resize(tf->nbrs->size);
    for(size_t i = 0; i < tf->nbrs->size; i++) {
        nbrs_numNeighbors[i] = tf->nbrs->numNeighbors[i];
    }
    nbrs_neighbors.resize(tf->nbrs->size);
    for(size_t i = 0; i < tf->nbrs->size; i++) {
        nbrs_neighbors[i].resize(nbrs_numNeighbors[i]);
        for(size_t j = 0; j < nbrs_numNeighbors[i]; j++) {
            nbrs_neighbors[i][j] = tf->nbrs->neighbors[i][j];
        }
    }

    harkio_TransferFunction_delete(&tf);

#endif  // #ifdef ENABLE_HARKIO3

    Pavg_sum.resize(SearchId.size());
    Pavg.resize(SearchId.size());
    Pavg_bin.resize(A[0].rows());
    for(size_t bins = 0; bins < A[0].rows(); bins++) {
        Pavg_bin[bins].resize(SearchId.size());
    }

    std::cerr << SearchId.size() << " srcs, " << A[0].cols() << " microphones, " << (A[0].rows() - 1) * 2 << " points" << std::endl;

    return true;
}

void LocalizationMUSIC::CalcAveragePower(std::vector<std::vector<float> >& w, std::vector<Matrix<std::complex<float> > >& E,
                                         std::vector<Matrix<std::complex<float> > >& A,
                                         std::vector<bool>& UsesFreq, int num_source,
                                         std::vector<float>& Pavg, std::vector<std::vector<float> >& Pavg_bin,
                                         std::vector<float>& Weight, bool enable_eigenvalue_weight) {
    int nh = Pavg.size();
    int num_mic = w[0].size();

    for(size_t ih = 0; ih < nh; ih++) {
        Pavg[ih] = 0.0f;
    }

    for(size_t k = 0; k < Pavg_bin.size(); k++) {
        for(size_t ih = 0; ih < Pavg_bin[0].size(); ih++) {
            Pavg_bin[k][ih] = 0.0f;
        }
    }

    float G = 0.0f;
    float norm_a = 0.0f;

    for(size_t k = 0; k < UsesFreq.size(); k++) {
        if(!UsesFreq[k]) {
            continue;
        }
        for(size_t ih = 0; ih < nh; ih++) {
            G = 0.0f;
            for(size_t m = num_mic - num_source - 1; m >= 0; m--) {
                G += norm(A[ih].getRow(k).transposed(true).elementwiseMultiplied(E[k].getColumn(m)).getElementSum());
                //G += norm(ublas::inner_prod(ublas::herm(ublas::row(A[ih], k)), ublas::column(E[k], m)));
            }
            //norm_a = ublas::inner_prod(ublas::herm(ublas::row(A[ih], k)), ublas::row(A[ih], k)).real();
            norm_a = A[ih].getRow(k).conjugated().elementwiseMultiplied(A[ih].getRow(k)).getElementSum().real();
            float EV_Weight = (enable_eigenvalue_weight ? sqrt(w[k][num_mic - 1]) : 1.0);
            Pavg[ih] += (norm_a / G) * Weight[k] * EV_Weight;
            Pavg_bin[k][ih] = 10 * log10((norm_a / G * Weight[k]) + 1.0f);
            std::cerr << "";
        }
    }
}

int LocalizationMUSIC::SearchLocation(std::vector<float>& Pavg_sum, std::vector<float>& Pavg, int n_source,
                                      std::vector<int>& direction, int mindeg, int maxdeg) {
    int n_result;
    int nh = Pavg.size();
    std::vector<int> candidate_id;
    std::vector<float> candidate_val;

    candidate_id.reserve(nh);
    candidate_id.resize(0);
    candidate_val.reserve(nh);
    candidate_val.resize(0);
    SearchPeak(Pavg_sum, candidate_id, candidate_val, mindeg, maxdeg);

    std::vector<int> index;
    for(size_t i = 0; i < candidate_id.size(); i++) {
        index.push_back(i);
    }

    std::sort(index.begin(), index.end(), std::bind<bool>(less_for_vec(), candidate_val, std::placeholders::_2, std::placeholders::_1));

    std::vector<int> res_id;
    for(size_t i = 0; i < index.size(); i++) {
        res_id.push_back(candidate_id[index[i]]);
    }

    std::vector<float> res_val;
    for(size_t i = 0; i < index.size(); i++) {
        res_val.push_back(candidate_val[index[i]]);
    }

    if(n_source == 0) {
        n_result = res_id.size();
    }
    else if(res_id.size() < n_source) {
        n_result = res_id.size();
    }
    else {
        n_result = n_source;
    }

    direction.resize(0);
    for(size_t i = 0; i < n_result; i++) {
        direction.push_back(res_id[i]);
    }

    return n_result;
}

void LocalizationMUSIC::SearchPeak(std::vector<float>& power, std::vector<int>& peak_cand_id, std::vector<float>& peak_cand_val,
                                   int mindeg, int maxdeg) {
    while(maxdeg - mindeg > 360) { maxdeg -= 360; }
    while(maxdeg - mindeg <= 0) { maxdeg += 360; }

    for(size_t ih = 0; ih < power.size(); ih++) {

        double x = (SearchId[ih][0] == -0.0) ? 0.0 : SearchId[ih][0];
        double y = (SearchId[ih][1] == -0.0) ? 0.0 : SearchId[ih][1];
        int deg = (int) (180.0 / M_PI * atan2(y, x));
        while(deg - mindeg > 360) { deg -= 360; }
        while(deg - mindeg <= 0) { deg += 360; }
        if((deg < mindeg) || (deg > maxdeg)) { continue; }
        float maxc = power[ih];
        bool pf = true;

        size_t i;
        for(i = 0; i < nbrs_ids.size(); i++) {
            if(nbrs_ids[i] == AId[ih]) {
                break;
            }
        }
        for(size_t j = 0; j < nbrs_numNeighbors[i]; j++) {
            size_t jh;
            for(jh = 0; jh < AId.size(); jh++) {
                if(nbrs_neighbors[i][j] == AId[jh]) {
                    break;
                }
            }
            if(maxc < power[jh]) {
                pf = false;
            }
        }
        if(pf) {
            peak_cand_id.push_back(ih);
            peak_cand_val.push_back(power[ih]);
        }
    }
}

#endif

/* end of file */
