
#ifndef ___LOCALIZATIONMUSIC_HPP___
#define ___LOCALIZATIONMUSIC_HPP___

#include <vector>
#include <algorithm>

#include "hark-lib/matrix.h"
#include "hark-lib/tf.h"

//using namespace hark;

namespace hark
{

//#include "hark-lib/node/LocalizationFunctionMUSIC.hpp"
//#include "hark-lib/node/LocalizationFunction.hpp"

void AddToCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq, Matrix<std::complex<float> >& X);
void NormalizeCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq, int sum_count);
void ClearCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq);
float MaxOfAbsValue(Matrix<std::complex<float> >& A);

/*
float MaxOfAbsValueFvector(std::vector<float>& A);
void mulfcomplex(std::complex<float>& a, std::complex<float>& b, std::complex<float>& c);
void addfcomplex(std::complex<float>& a, std::complex<float>& b, std::complex<float>& c);
void subfcomplex(std::complex<float>& a, std::complex<float>& b, std::complex<float>& c);
void devfcomplex(std::complex<float>& a, std::complex<float>& b, std::complex<float>& c);
void mulMAT(Matrix<std::complex<float> >& a, Matrix<std::complex<float> >& b, Matrix<std::complex<float> >& c);
void invMAT(Matrix<std::complex<float> >& a, Matrix<std::complex<float> >& c);
void display_fcmat(Matrix<std::complex<float> >& a);
void read_fcmat(char *name1, char *name2, Matrix<std::complex<float> >& a);
int save_mat_fcmat(char *name1, char *name2, Matrix<std::complex<float> >& a);
int save_mat_fcmat_real(char *name1, Matrix<std::complex<float> >& a);
void norm_display_fcmat(Matrix<std::complex<float> >& a);
void sort_eig_fvec_ascending(std::vector<float>& ai, Matrix<std::complex<float> >& bi, std::vector<float>& af, Matrix<std::complex<float> >& bf);
void sort_eig_fcvec_ascending(std::vector<std::complex<float> >& ai, Matrix<std::complex<float> >& bi, std::vector<float>& af, Matrix<std::complex<float> >& bf);
void sort_eig_fvec_descending(std::vector<float>& ai, Matrix<std::complex<float> >& bi, std::vector<float>& af, Matrix<std::complex<float> >& bf);
void sort_eig_fcvec_descending(std::vector<std::complex<float> >& ai, Matrix<std::complex<float> >& bi, std::vector<float>& af, Matrix<std::complex<float> >& bf);
*/

class LocalizationMUSIC {
    int NumMicrophones;
    int TimeLength;
    int Shift;
    int FreqLength;
    int SamplingRate;
    std::vector<bool> UsesFreq;

    int maxnum_output_peaks;

    //AMatrixArray1D A;
    std::vector<Matrix<std::complex<float> > > A;
    std::vector<int> AId;

    std::vector<std::vector<float> > SearchId;

    std::vector<int> nbrs_ids;
    std::vector<int> nbrs_numNeighbors;
    std::vector<std::vector<int> > nbrs_neighbors;

    std::vector<Matrix<std::complex<float> > > Rxx;
    std::vector<float> MaxRxx;
    std::vector<Matrix<std::complex<float> > > RxxN;
    std::vector<float> MaxRxxN;
    std::vector<Matrix<std::complex<float> > > E;
    std::vector<Matrix<std::complex<float> > > EN;

    int SumCount;
    Matrix<std::complex<float> > R;
    Matrix<std::complex<float> > RN;
    Matrix<std::complex<float> > V;
    Matrix<std::complex<float> > VN;

    Matrix<std::complex<float> > TempCalc1;
    Matrix<std::complex<float> > TempCalc2;
    Matrix<std::complex<float> > TempCalc3;
    Matrix<std::complex<float> > TempCalc4;
    Matrix<std::complex<float> > TempCalc5;
    Matrix<std::complex<float> > TempCalc6;
    Matrix<std::complex<float> > TempCalc7;
    Matrix<std::complex<float> > TempCalc8;
    Matrix<std::complex<float> > TempCalc9;

    std::vector<std::vector<float> > EigenValue;
    std::vector<float>  REigenValue;
    std::vector<std::complex<float> > CEigenValue;

    std::vector<float> Pavg;
    std::vector<float> Pavg_sum;
    std::vector<std::vector<float> > Pavg_bin;

    int NumSource;
    int FoundSourceCount;

    std::vector<int> FoundId;

public:
    LocalizationMUSIC(int num_mic, int sampling_rate, int tlength, int shift, int maxn_out_peaks);

    void SetNumSource(int num_source) { NumSource = num_source; }
    void SetUsedFrequency(int lo, int up);
    const std::vector<bool>& GetUsedFrequency() { return UsesFreq; }
    void ClearUsedFrequency() { std::fill(UsesFreq.begin(), UsesFreq.end(), false); }

    void 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);

    void CalcAveragePower(std::vector<std::vector<float> >& w,
                          std::vector<Matrix<std::complex<float> > >& E,
                          //AMatrixArray1D& A,
                          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 SearchLocation(std::vector<float>& Pavg_sum, std::vector<float>& Pavg,
                       int n_source, std::vector<int>& direction, 
                       int mindeg, int maxdeg);

    void SearchPeak(std::vector<float>& power, 
                    std::vector<int>& peak_cand_id, 
                    std::vector<float>& peak_cand_val, 
                    int mindeg, int maxdeg);

    int GetTFNumChannels(char* filename);

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

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

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

    int GetFoundSourceCount() { return FoundSourceCount; }

    const std::vector<int>& GetFoundId() { return FoundId; }

    const std::vector<std::vector<float> >& GetSearchId() { return SearchId; }

    float GetP(int iid) { return Pavg_sum[iid]; }
    float GetP_bin(int bin, int iid) { return Pavg_bin[bin][iid]; }

    float GetEigenValue(int bin, int nm) { return EigenValue[bin][nm] * MaxRxx[bin] / MaxRxxN[bin]; }
};

}

#endif //___LOCALIZATIONMUSIC_HPP___
