
#include <iostream>
#include <fstream>

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

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

//#ifdef ENABLE_LAPACK

void AddToCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq, Matrix<std::complex<float> >& X)
{
    for(size_t k = 0; k < UsesFreq.size(); k++) {
        if(UsesFreq[k]) {
            Rxx[k] += X.col(k) *  X.col(k).transpose();
            //Rxx[k] += outer_prod(ublas::column(X, k), ublas::herm(ublas::column(X, k)));
        }
    }
}

void NormalizeCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq, int sum_count)
{
    float sum_count_1 = 1.0f / sum_count;
    for(size_t k = 0; k < UsesFreq.size(); k++) {
        if(UsesFreq[k]) {
            Rxx[k] *= sum_count_1;
        }
    }
}

void ClearCorrelation(std::vector<Matrix<std::complex<float> > >& Rxx, const std::vector<bool>& UsesFreq)
{
    for(size_t k = 0; k < UsesFreq.size(); k++) {
        //for(size_t i = 0; i < Rxx[k].rows(); i++){
        //    for(size_t j = 0; j < Rxx[k].cols(); j++){
        //        Rxx[k](i, j) = 0.0f;
        //    }
        //}
        Rxx[k].setZero();
    }
}

float MaxOfAbsValue(Matrix<std::complex<float> >& A)
{
    float max = 0.0f;
    for(size_t i = 0; i < A.rows(); i++) {
        for(size_t j = 0; j < A.cols(); j++) {
            if(std::abs(A(i, j).real()) > max) {
                max = std::abs(A(i, j).real());
            }
        }
    }
    return max;
}





/*
#else

void ReadTransferFunction(const char* filename,
                          AMatrixArray& A,
                          std::vector<int>& search_dir,
                          std::vector<int>& search_range)
{
  fstream fin;
  fin.open(filename, ios::in);
  if(fin.fail()){
      cout << string("Can't open file'")+filename+"'." << endl;
      exit(1);
  }

  int nd;
  int nr;
  int nmic;
  int nfft;

  fin.read((char*)&nd, sizeof(int));
  fin.read((char*)&nr, sizeof(int));
  fin.read((char*)&nmic, sizeof(int));
  fin.read((char*)&nfft, sizeof(int));

  int slength = nfft / 2 + 1;

  cerr << nd << " directions, " << nr << " ranges, " << nmic << " microphones, ";
  cerr << nfft << " points" << endl;
  A.resize(extents[nd][nr]);
  for (int id = 0; id < nd; id++) {
    for (int ir = 0; ir < nr; ir++) {
      A[id][ir].resize(slength, nmic);
    }
  }

  search_dir.resize(nd);
  search_range.resize(nr);
  fin.read((char*)&search_dir[0], nd * sizeof(int));
  fin.read((char*)&search_range[0], nr * sizeof(int));

  vector<complex<float> > buf(slength);
  for (int id = 0; id < nd; id++) {
    for (int ir = 0; ir < nr; ir++) {
      for (int m = 0; m < nmic; m++) {
        fin.read((char*)&buf[0], slength * sizeof(complex<float>));

        for (int k = 0; k < slength; k++) {
          A[id][ir](k, m) = buf[k];
        }
      }
    }
  }
}



void CalcAveragePower(std::vector<std::vector<float> >& w,
                      std::vector<Matrix<std::complex<float> > >& E,
                      AMatrixArray& A,
                      std::vector<bool>& UsesFreq,
                      int num_source, std::vector<std::vector<float> >& Pavg)
{
  int nd = Pavg.size();
  int nr = Pavg[0].size();
  int num_mic = w[0].size();
    
  for (int id = 0; id < nd; id++) {
    for (int ir = 0; ir < nr; ir++) {
      Pavg[id][ir] = 0.0f;
    }
  }

  float G = 0.0f;
  float norm_a = 0.0f;
  static fcmatrix I(ublas::identity_matrix<complex<float> >(num_mic, num_mic));
  for (int k = 0; k < UsesFreq.size(); k++) {
    if (!UsesFreq[k]) {
      continue;
    }
        
    ublas::matrix_range<ublas::matrix<complex<float> > >
      E_ss(E[k], ublas::range(0, num_mic), ublas::range(0, num_source));
    Matrix<std::complex<float> > E_nn = I - prod(E_ss, herm(E_ss));
    for (int id = 0; id < nd; id++) {
      for (int ir = 0; ir < nr; ir++) {
        G = 0.0f;

    // OLD SLOW ALGORITHM
        for (int m = num_mic - num_source - 1; m >= 0; m--) {
          G += norm(ublas::inner_prod(ublas::herm(ublas::row(A[id][ir], k)),
                                      ublas::column(E[k], m)));
        }
        norm_a = ublas::inner_prod(ublas::herm(ublas::row(A[id][ir], k)),
                                   ublas::row(A[id][ir], k)).real();
        Pavg[id][ir] += (norm_a / G) * sqrt(w[k][num_mic - 1]);

*/
    /*
    // NEW FAST ALGORITHM but Different result ...
        G = (inner_prod(herm(ublas::row(A[id][ir], k)),
                        prod(E_nn, ublas::row(A[id][ir], k)))).real();
                            
        norm_a = ublas::inner_prod(ublas::herm(ublas::row(A[id][ir], k)),
                                   ublas::row(A[id][ir], k)).real();

        Pavg[id][ir] += (norm_a / G) * sqrt(w[k][0]);
    */
/*
      }
    }
  }
}

void SearchPeak(std::vector<float>& power, std::vector<int>& peak_cand)
{
  float p1;
  float p2;
  float p3;

  if (power.size() < 2) {
    peak_cand.push_back(0);
  }
  else {
    for (int i = 0; i < power.size(); i++) {
      p1 = (i == 0 ? power.back() : power[i - 1]);
      p2 = power[i];
      p3 = (i == power.size() - 1 ? power.front() : power[i + 1]);

      if (p2 - p1 > 0 && p3 - p2 < 0) {
        peak_cand.push_back(i);
      }
    }
  }
}


int SearchLocation(std::vector<float>& Pavg_sum, std::vector<std::vector<float> >& Pavg,
                   int n_source, std::vector<int>& direction, std::vector<int>& range)
{
  int n_candidate;
  int n_result;
  int nd = Pavg.size();
  int nr = Pavg[0].size();
  std::vector<int> candidate;
  std::vector<int> index;
  std::vector<float> tmp(nd + nr);
  float tmp_min;
  float tmp_max;

  candidate.reserve(nd + nr);

  candidate.resize(0);
  SearchPeak(Pavg_sum, candidate);

  if (candidate.size() < n_source) {
    //n_result = n_candidate;
    n_result = candidate.size(); //modified by shohei 08/02/13
  }
  else {
    n_result = n_source;
  }

  partial_sort(candidate.begin(),
               candidate.begin() + n_result,
               candidate.end(),
               std::bind<bool>(less_for_vec(), Pavg_sum, _2, _1));
  copy(candidate.begin(), candidate.begin() + n_result, direction.begin());

  candidate.resize(0);
  for (int source = 0; source < n_result; source++) {
    int id = direction[source];
    SearchPeak(Pavg[id], candidate);
    std::vector<int>::iterator it =
      max_element(candidate.begin(), candidate.end(),
                  std::bind<bool>(less_for_vec(), Pavg_sum, _1, _2));
    range[source] = *it;
  }

  return n_result;
}

#endif
*/

/* end of file */
