/***************************************************************************
**
**  This file is part of gpfksimulator.
**
**  gpfksimulator is free software: you can redistribute it and/or modify
**  it under the terms of the GNU General Public License as published by
**  the Free Software Foundation, either version 3 of the License, or
**  (at your option) any later version.
**
**  gpfksimulator is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.
**
**  You should have received a copy of the GNU General Public License
**  along with Foobar.  If not, see <http://www.gnu.org/licenses/>
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2007-11-13
**  Copyright: 2007-2019
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <ArrayCore.h>

#include "FKArrayMap.h"
#include "MediumParameters.h"
#include "SourceParameters.h"

/*!
  \class FKArrayMap FKArrayMap.h
  \brief Brief description of class still missing

  Full description of class still missing
*/

/*!
  Description of constructor still missing
*/
FKArrayMap::FKArrayMap()
    : AbstractFunction()
{
  TRACE;
  _polarization=FK_Vertical;
  _waveModel=SourceParameters::PlaneWaves;
  _sensorRotation=0.0;
  _kmax=0.0;

  _stationSig=0;

  _rotateStepCount=360;
  _rotateStep=2.0*M_PI/(double)_rotateStepCount;
  _rayleighCrossSpectra=new ComplexMatrix[_rotateStepCount];;
  _radialCrossSpectra=new ComplexMatrix[_rotateStepCount];;
  _transverseCrossSpectra=new ComplexMatrix[_rotateStepCount];;
}

/*!
  Description of destructor still missing
*/
FKArrayMap::~FKArrayMap()
{
  TRACE;
  clearSignals();
  delete [] _rayleighCrossSpectra;
  delete [] _radialCrossSpectra;
  delete [] _transverseCrossSpectra;
  qDeleteAll(_sourceSig);
}

void FKArrayMap::setSourceSignals(int iSrc, const SourceParameters& src,
                                  const MediumParameters& medium, int blockCount)
{
  TRACE;
  _sourceSig[iSrc]->setParameters(_waveModel, src, medium, blockCount, _stations);
}

void FKArrayMap::setStations(const QVector<Point2D>& p)
{
  TRACE;
  _stations=p;
  uniquePoints();
}

void FKArrayMap::uniquePoints()
{
  TRACE;
  int i, j, is=0, n=_stations.count();
  Point * pList=new Point[ n ];
  for(i=0;i < n;i++ )
    pList[ i ]=_stations[ i ];
  for(i=0;i < n;i++ ) {
    for(j=i + 1;j < n;j++ ) {
      if(pList[ i ]==pList[ j ] )
        break;
    }
    if(j==n) {
      _stations[ is ]=pList[ i ];
      is++;
    } else
      _stations.pop_back();
  }
  delete [] pList;
}

/*!
  Calculate kmax from minimum distance between stations
*/
double FKArrayMap::theoreticalKmax() const
{
  TRACE;
  KminSolver s(_stations);
  double dmin, dmax;
  s.distanceRange(dmin, dmax);
  // k=2*M_PI/lambda, due to Nyquist, d<lambda/2
  return M_PI/dmin;
}

double FKArrayMap::value(const Point& k) const
{
  TRACE;
  if(_kmax>0.0) {
    if(k.Point2D::abs2()>_kmax*_kmax) {
      return 0.0;
    }
  }
  switch(_polarization) {
  case FK_Vertical:
    break;
  case FK_Radial:
    return FKRadial(k);
  case FK_Transverse:
    return FKTransverse(k);
  case FK_Rayleigh:
    return FKRayleigh(k);
  case FK_RayleighPositive:
    return FKRayleighPositive(k);
  case FK_RayleighNegative:
    return FKRayleighNegative(k);
  case FK_EllipticitySign:
    return FKEllipticitySign(k);
  case HRFK_Vertical:
    return HRFKVertical(k);
  case HRFK_Radial:
    return HRFKRadial(k);
  case HRFK_Transverse:
    return HRFKTransverse(k);
  case HRFK_Rayleigh:
    return HRFKRayleigh(k);
  case HRFK_Ellipticity:
    return HRFKEllipticity(k);
  case HRFK_RayleighNoise:
    return HRFKRayleighNoise(k);
  case HRFK_PoggiEllipticity:
    return HRFKPoggiEllipticity(k);
  case HRFK2_Rayleigh:
    return HRFK2Rayleigh(k);
  case HRFK2_Ellipticity:
    return HRFK2Ellipticity(k);
  case HRFK2_AbsEllipticity:
    return fabs(HRFK2Ellipticity(k));
  case HRFK_DirectLove:
    return HRFKDirectLove(k);
  case HRFK_DirectRayleigh:
    return HRFKDirectRayleigh(k);
  case HRFK_DirectEllipticity:
    return HRFKDirectEllipticity(k);
  case HRFK_DirectAbsEllipticity:
    return fabs(HRFKDirectEllipticity(k));
  case HRFK2_DirectRayleigh:
    return HRFK2DirectRayleigh(k);
  case HRFK2_DirectEllipticity:
    return HRFK2DirectEllipticity(k);
  case HRFK2_DirectAbsEllipticity:
    return fabs(HRFK2DirectEllipticity(k));
  }
  return FKVertical(k);
}

double FKArrayMap::thirdMinimum() const
{
  switch(_polarization) {
  case FK_Vertical:
  case FK_Radial:
  case FK_Transverse:
  case FK_RayleighPositive:
  case FK_RayleighNegative:
  case HRFK_Vertical:
  case HRFK_Radial:
  case HRFK_Transverse:
  case FK_EllipticitySign:
  case HRFK_Ellipticity:
  case HRFK_RayleighNoise:
  case HRFK_PoggiEllipticity:
  case HRFK2_Ellipticity:
  case HRFK2_AbsEllipticity:
  case HRFK_DirectLove:
  case HRFK_DirectEllipticity:
  case HRFK_DirectAbsEllipticity:
  case HRFK2_DirectEllipticity:
  case HRFK2_DirectAbsEllipticity:
    break;
  case FK_Rayleigh:
    return -1.0;
  case HRFK_Rayleigh:
  case HRFK2_Rayleigh:
  case HRFK_DirectRayleigh:
  case HRFK2_DirectRayleigh:
    return -2.5;
  }
  return 0.0;
}

double FKArrayMap::thirdMaximum() const
{
  switch(_polarization) {
  case FK_Vertical:
  case FK_Radial:
  case FK_Transverse:
  case FK_RayleighPositive:
  case FK_RayleighNegative:
  case HRFK_Vertical:
  case HRFK_Radial:
  case HRFK_Transverse:
  case FK_EllipticitySign:
  case HRFK_Ellipticity:
  case HRFK_RayleighNoise:
  case HRFK_PoggiEllipticity:
  case HRFK2_Ellipticity:
  case HRFK2_AbsEllipticity:
  case HRFK_DirectLove:
  case HRFK_DirectEllipticity:
  case HRFK_DirectAbsEllipticity:
  case HRFK2_DirectEllipticity:
  case HRFK2_DirectAbsEllipticity:
    break;
  case FK_Rayleigh:
    return 1.0;
  case HRFK_Rayleigh:
  case HRFK2_Rayleigh:
  case HRFK_DirectRayleigh:
  case HRFK2_DirectRayleigh:
    return 2.5;
  }
  return 0.0;
}

double FKArrayMap::thirdStep() const
{
  switch(_polarization) {
  case FK_Vertical:
  case FK_Radial:
  case FK_Transverse:
  case FK_RayleighPositive:
  case FK_RayleighNegative:
  case HRFK_Vertical:
  case HRFK_Radial:
  case HRFK_Transverse:
  case FK_EllipticitySign:
  case HRFK_Ellipticity:
  case HRFK_RayleighNoise:
  case HRFK_PoggiEllipticity:
  case HRFK2_Ellipticity:
  case HRFK2_AbsEllipticity:
  case HRFK_DirectLove:
  case HRFK_DirectEllipticity:
  case HRFK_DirectAbsEllipticity:
  case HRFK2_DirectEllipticity:
  case HRFK2_DirectAbsEllipticity:
    break;
  case FK_Rayleigh:
    return 2.0;
  case HRFK_Rayleigh:
  case HRFK2_Rayleigh:
  case HRFK_DirectRayleigh:
  case HRFK2_DirectRayleigh:
    return 0.05;
  }
  return 1.0;
}

void FKArrayMap::crossPowerVertical(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
  ComplexMatrix sig(stationCount, 1);
  ComplexMatrix& covmat=_verticalCrossSpectrum;
  covmat.resize(stationCount);
  covmat.zero(0.0);

  for(int iBlock=0; iBlock<blockCount; iBlock++) {
    for(int iStat=0; iStat<stationCount; iStat++) {
      Complex& s=sig.at(iStat, 0);
      s=_stationSig[2][iStat][iBlock];
      if(normalize) {
        s/=s.abs();
      }
    }
    covmat+=sig*sig.conjugate().transposed();
  }
  covmat*=1.0/(double)blockCount;
  //QTextStream(stdout) << covmat.toOctaveString() << ::endl;
  //QTextStream(stdout) << sig.toOctaveString() << ::endl;
  // Add white noise
  int n=covmat.rowCount();
  double total=1.0+noiseRatio;
  for(int i=0; i<n; i++) {
    covmat.at(i, i)*=total;
  }
}

/*!
  Test of arrays with optical fiber with horizontal sensors along the fiber only.
  Test with circular arrays only.
*/
QVector<Angle> FKArrayMap::horizontalOrientations()
{
  int stationCount=_stations.count();
  QVector<Angle> orientations(stationCount);
  // Calculate orientation of each station
  for(int is=0; is<stationCount; is++) {
    Point2D p=_stations.at(is);
    p-=Point2D(2000, 2000);
    Angle& orientation=orientations[is];
    orientation.set(p.x(), p.y());
    orientation.chSign();
    orientation.initRadians();
  }
  return orientations;
}

//#define OPTICAL_FIBER_ARRAY

void FKArrayMap::crossPowerRadial(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
#ifdef OPTICAL_FIBER_ARRAY
  QVector<Angle> orientations=horizontalOrientations();
  Complex north, east;
#endif
  ComplexMatrix sig(stationCount, 1);
  for(int rotationIndex=0; rotationIndex<_rotateStepCount; rotationIndex++) {
    ComplexMatrix& covmat=_radialCrossSpectra[rotationIndex];
    covmat.resize(stationCount);
    covmat.zero(0.0);
    Angle rotation;
    rotation.setRadians(_rotateStep*rotationIndex);
    for(int iBlock=0; iBlock<blockCount; iBlock++) {
      for(int iStat=0; iStat<stationCount; iStat++) {
        Complex& s=sig.at(iStat, 0);
#ifdef OPTICAL_FIBER_ARRAY
        // North is along fiber, east is perpendicular
        //north=_stationSig[1][iStat][iBlock]*orientations[iStat].cos()-_stationSig[0][iStat][iBlock]*orientations[iStat].sin();
        //east=_stationSig[0][iStat][iBlock]*orientations[iStat].cos()+_stationSig[1][iStat][iBlock]*orientations[iStat].sin();
        //east=0.0;
        //s=rotation.cos()*(orientations[iStat].cos()*east-orientations[iStat].sin()*north);
        //s+=rotation.sin()*(orientations[iStat].cos()*north+orientations[iStat].sin()*east);
        // Grid array with only east
        if(iStat%2==1) {
          s=rotation.cos()*_stationSig[0][iStat][iBlock];
        } else {
          s=rotation.sin()*_stationSig[1][iStat][iBlock];
        }
#else
        s=rotation.cos()*_stationSig[0][iStat][iBlock];
        s+=rotation.sin()*_stationSig[1][iStat][iBlock];
#endif
        if(normalize) {
          s/=s.abs();
        }
      }
      covmat+=sig*sig.conjugate().transposed();
    }
    covmat*=1.0/(double)blockCount;
    // Add white noise
    int n=covmat.rowCount();
    double p=covmat.trace(Complex::null).re();
    p*=noiseRatio;
    for(int i=0; i<n; i++) {
      covmat.at(i, i)+=p;
    }
  }
}

void FKArrayMap::crossPowerTransverse(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
#ifdef OPTICAL_FIBER_ARRAY
  QVector<Angle> orientations=horizontalOrientations();
  Complex north, east;
#endif
  ComplexMatrix sig(stationCount, 1);
  for(int rotationIndex=0; rotationIndex<_rotateStepCount; rotationIndex++) {
    ComplexMatrix& covmat=_transverseCrossSpectra[rotationIndex];
    covmat.resize(stationCount);
    covmat.zero(0.0);
    Angle rotation;
    rotation.setRadians(_rotateStep*rotationIndex);
    for(int iBlock=0; iBlock<blockCount; iBlock++) {
      for(int iStat=0; iStat<stationCount; iStat++) {
        Complex& s=sig.at(iStat, 0);
#ifdef OPTICAL_FIBER_ARRAY
        // North is along fiber, east is perpendicular (circle)
        //north=_stationSig[1][iStat][iBlock]*orientations[iStat].cos()-_stationSig[0][iStat][iBlock]*orientations[iStat].sin();
        //east=_stationSig[0][iStat][iBlock]*orientations[iStat].cos()+_stationSig[1][iStat][iBlock]*orientations[iStat].sin();
        //east=0.0;
        //s=rotation.cos()*(orientations[iStat].cos()*north+orientations[iStat].sin()*east);
        //s-=rotation.sin()*(orientations[iStat].cos()*east-orientations[iStat].sin()*north);
        // Grid array with only east
        if(iStat%2==1) {
          s=rotation.cos()*_stationSig[1][iStat][iBlock];
        } else {
          s=-rotation.sin()*_stationSig[0][iStat][iBlock];
        }
#else
        s=rotation.cos()*_stationSig[1][iStat][iBlock];
        s-=rotation.sin()*_stationSig[0][iStat][iBlock];
#endif
        if(normalize) {
          s/=s.abs();
        }
      }
      covmat+=sig*sig.conjugate().transposed();
    }
    covmat*=1.0/(double)blockCount;
    // Add white noise
    int n=covmat.rowCount();
    double p=covmat.trace(Complex::null).re();
    p*=noiseRatio;
    for(int i=0; i<n; i++) {
      covmat.at(i, i)+=p;
    }
  }
}

void FKArrayMap::crossPowerRayleigh(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  ComplexMatrix sig(stationCount2, 1);
  for(int rotationIndex=0; rotationIndex<_rotateStepCount; rotationIndex++) {
    ComplexMatrix& covmat=_rayleighCrossSpectra[rotationIndex];
    covmat.resize(stationCount2);
    covmat.zero(0.0);
    Angle rotation;
    rotation.setRadians(_rotateStep*rotationIndex);
    for(int iBlock=0; iBlock<blockCount; iBlock++) {
      for(int iStat=0; iStat<stationCount; iStat++) {
        Complex& s=sig.at(iStat, 0);
        s=rotation.cos()*_stationSig[0][iStat][iBlock];
        s+=rotation.sin()*_stationSig[1][iStat][iBlock];
        if(normalize) {
          s/=s.abs();
        }
      }
      for(int iStat=0; iStat<stationCount; iStat++) {
        Complex& s=sig.at(stationCount+iStat, 0);
        s=_stationSig[2][iStat][iBlock];
        if(normalize) {
          s/=s.abs();
        }
      }
      covmat+=sig*sig.conjugate().transposed();
    }
    covmat*=1.0/(double)blockCount;
    /*if(rotationIndex==0) {
      QTextStream(stdout) << covmat.toOctaveString() << ::endl;
    }*/
    // Add white noise
    int n=covmat.rowCount();
    double total=1.0+noiseRatio;
    for(int i=0; i<n; i++) {
      covmat.at(i, i)*=total;
    }
    // Distinct noise ratio for horizontal and vertical (as requested by a reviewer)
    /*double total=1.0+noiseRatio*3;
    for(int i=0; i<stationCount; i++) {
      covmat.at(i, i)*=total;
    }
    total=1.0+noiseRatio;
    for(int i=0; i<stationCount; i++) {
      covmat.at(stationCount+i, stationCount+i)*=total;
    }*/
  }
}

void FKArrayMap::crossPowerDirectLove(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  ComplexMatrix sig(stationCount2, 1);
  ComplexMatrix& covmat=_loveCrossSpectrum;
  covmat.resize(stationCount2);
  covmat.zero(0.0);
  for(int iBlock=0; iBlock<blockCount; iBlock++) {
    for(int iStat=0; iStat<stationCount; iStat++) {
      for(int iComp=0; iComp<2; iComp++) {
        Complex& s=sig.at(iStat+iComp*stationCount, 0);
        s=_stationSig[iComp][iStat][iBlock];
        if(normalize) {
          s/=s.abs();
        }
      }
    }
    covmat+=sig*sig.conjugate().transposed();
  }
  covmat*=1.0/(double)blockCount;
  /*if(rotationIndex==0) {
    QTextStream(stdout) << covmat.toOctaveString() << ::endl;
  }*/
  // Add white noise
  int n=covmat.rowCount();
  double total=1.0+noiseRatio;
  for(int i=0; i<n; i++) {
    covmat.at(i, i)*=total;
  }
}

void FKArrayMap::crossPowerDirectRayleigh(int blockCount, bool normalize, double noiseRatio)
{
  TRACE;
  int stationCount=_stations.count();
  int stationCount3=3*stationCount;
  ComplexMatrix sig(stationCount3, 1);
  ComplexMatrix& covmat=_rayleighCrossSpectrum;
  covmat.resize(stationCount3);
  covmat.zero(0.0);
  for(int iBlock=0; iBlock<blockCount; iBlock++) {
    for(int iStat=0; iStat<stationCount; iStat++) {
      for(int iComp=0; iComp<3; iComp++) {
        Complex& s=sig.at(iStat+iComp*stationCount, 0);
        s=_stationSig[iComp][iStat][iBlock];
        if(normalize) {
          s/=s.abs();
        }
      }
    }
    covmat+=sig*sig.conjugate().transposed();
  }
  covmat*=1.0/static_cast<double>(blockCount);
  /*if(rotationIndex==0) {
    QTextStream(stdout) << covmat.toOctaveString() << ::endl;
  }*/
  // Add white noise
  int n=covmat.rowCount();
  double total=1.0+noiseRatio;
  for(int i=0; i<n; i++) {
    covmat.at(i, i)*=total;
  }
}

bool FKArrayMap::inverseCrossPower(ComplexMatrix& covmat)
{
  TRACE;
  bool svd=false;
  if(svd) {
    ComplexMatrix u, vt;
    QVector<double> sigma;
    covmat.singularValue(sigma, u, vt);
    App::log(QString("singular value[%1]= %2\n").arg(0).arg(sigma[0]));
    //double noise=sigma[0]*1e-16*sigma.count();
    //double noise=sigma[0]*1e-4;
    //double noise=std::numeric_limits<double>::infinity();
    double noise=0;
    for(int i=sigma.count()-1; i>0; i--) {
      App::log(QString("singular value[%1]= %2\n").arg(i).arg(sigma[i]));
      if(sigma[i]<noise) {
        sigma[i]=0.0;
      }
    }
    return covmat.pseudoInvert(u, sigma, vt);
  } else {
    if(covmat.choleskyInvert()) {
      return true;
    } else {
      App::log(tr("Cannot invert cross spectrum\n") );
      covmat.zero(0.0);
      return false;
    }
  }
}

bool FKArrayMap::inverseCrossPower(ComplexMatrix * covmats)
{
  TRACE;
  for(int rotationIndex=0; rotationIndex<_rotateStepCount; rotationIndex++) {
    if(!inverseCrossPower(covmats[rotationIndex])) {
      App::log(tr("Cannot invert cross spectrum for rotation index %1\n").arg(rotationIndex) );
      for(rotationIndex++; rotationIndex<_rotateStepCount; rotationIndex++) {
        covmats[rotationIndex].zero(0.0);
      }
      return false;
    }
  }
  return true;
}

void FKArrayMap::steering(const Point& k,
                          ComplexMatrix& e, ComplexMatrix& eh) const
{
  int stationCount=_stations.count();
  e.resize(stationCount, 1);
  eh.resize(1, stationCount);
  Complex c;
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    c.setUnitExp(-r.scalarProduct(k));
    e.at(i, 0)=c;
    eh.at(0, i)=conjugate(c);
  }
}

void FKArrayMap::FKRayleighSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh, double ell) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  e.resize(stationCount2, 1);
  eh.resize(1, stationCount2);
  Complex c;
  if(fabs(ell)<1.0) {
    Complex cell(0.0, -1.0/ell);
    for(int i=stationCount-1; i>=0; i--) {
      const Point2D& r=_stations[i];
      c.setUnitExp(-r.scalarProduct(k));
      e.at(i, 0)=cell*c;
      e.at(i+stationCount, 0)=c;
      eh.at(0, i)=conjugate(cell*c);
      eh.at(0, i+stationCount)=conjugate(c);
    }
  } else {
    Complex cell(0.0, ell);
    for(int i=stationCount-1; i>=0; i--) {
      const Point2D& r=_stations[i];
      c.setUnitExp(-r.scalarProduct(k));
      e.at(i, 0)=c;
      e.at(i+stationCount, 0)=cell*c;
      eh.at(0, i)=conjugate(c);
      eh.at(0, i+stationCount)=conjugate(cell*c);
    }
  }
}

void FKArrayMap::HRFKRayleighRadialSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh, double ell) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  e.resize(stationCount2, 1);
  eh.resize(1, stationCount2);
  Complex q;
  Complex cell(0.0, -ell);
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    q.setUnitExp(-r.scalarProduct(k));
    e.at(i+stationCount, 0)=q;
    eh.at(0, i+stationCount)=conjugate(q);
    q*=cell;
    e.at(i, 0)=q;
    eh.at(0, i)=conjugate(q);
  }
}

void FKArrayMap::HRFKRayleighVerticalSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh, double ell) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  e.resize(stationCount2, 1);
  eh.resize(1, stationCount2);
  Complex q;
  Complex cell(0.0, 1.0/ell);
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    q.setUnitExp(-r.scalarProduct(k));
    e.at(i, 0)=q;
    eh.at(0, i)=conjugate(q);
    q*=cell;
    e.at(i+stationCount, 0)=q;
    eh.at(0, i+stationCount)=conjugate(q);
  }
}

void FKArrayMap::HRFK2RayleighSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh, double ell) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  e.resize(stationCount2, 1);
  eh.resize(1, stationCount2);
  Complex c;
  Complex cell(0.0, -ell);
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    c.setUnitExp(-r.scalarProduct(k));
    e.at(i, 0)=cell*c;
    e.at(i+stationCount, 0)=c;
    eh.at(0, i)=conjugate(cell*c);
    eh.at(0, i+stationCount)=conjugate(c);
  }
}

void FKArrayMap::HRFKDirectLoveSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  e.resize(stationCount2, 1);
  eh.resize(1, stationCount2);
  Complex c;
  Angle rotation(k.x(), k.y());
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    c.setUnitExp(-r.scalarProduct(k));
    e.at(i, 0)=-rotation.sin()*c;
    e.at(i+stationCount, 0)=rotation.cos()*c;
    eh.at(0, i)=conjugate(e.at(i, 0));
    eh.at(0, i+stationCount)=conjugate(e.at(i+stationCount, 0));
  }
}

void FKArrayMap::HRFKDirectRayleighSteering(const Point& k, ComplexMatrix& e, ComplexMatrix& eh, double ell) const
{
  int stationCount=_stations.count();
  int stationCount2=2*stationCount;
  int stationCount3=3*stationCount;
  e.resize(stationCount3, 1);
  eh.resize(1, stationCount3);
  Complex c;
  Angle rotation(k.x(), k.y());
  Complex cell(0.0, -ell);
  Complex cc;
  for(int i=stationCount-1; i>=0; i--) {
    const Point2D& r=_stations[i];
    c.setUnitExp(-r.scalarProduct(k));
    cc=cell*c;
    e.at(i, 0)=rotation.cos()*cc;
    e.at(i+stationCount, 0)=rotation.sin()*cc;
    e.at(i+stationCount2, 0)=c;
    eh.at(0, i)=conjugate(e.at(i, 0));
    eh.at(0, i+stationCount)=conjugate(e.at(i+stationCount, 0));
    eh.at(0, i+stationCount2)=conjugate(e.at(i+stationCount2, 0));
  }
}

inline double FKArrayMap::fkPower(const ComplexMatrix& covmat, const ComplexMatrix& e, const ComplexMatrix& eh) const
{
  ComplexMatrix p=eh;
  p*=covmat;
  p*=e;
  ASSERT(p.columnCount()==1 && p.rowCount()==1);
  if(fabs(p.at(0, 0).im())>1e-8*fabs(p.at(0, 0).re())) {
    App::log(tr("Imaginary part not null: %1 %2\n").arg(p.at(0, 0).re()).arg(p.at(0, 0).im()));
  }
  return p.at(0, 0).re();
}

inline double FKArrayMap::fkPower(const Point& k, const ComplexMatrix& covmat) const
{
  ComplexMatrix e, eh;
  steering(k, e, eh);
  return fkPower(covmat, e, eh);
}

inline double FKArrayMap::hrfkPower(const Point& k, const ComplexMatrix& covmat) const
{
  return 1.0/fkPower(k, covmat);
}

inline double FKArrayMap::hrfkPower(const ComplexMatrix& covmat, const ComplexMatrix& e, const ComplexMatrix& eh) const
{
  return 1.0/fkPower(covmat, e, eh);
}

double FKArrayMap::FKVertical(const Point& k) const
{
  return fkPower(k, _verticalCrossSpectrum)/(_stations.count()*_stations.count());
}

double FKArrayMap::FKRadial(const Point& k) const
{
  return fkPower(k, _radialCrossSpectra[rotationIndex(k)])/(_stations.count()*_stations.count());
}

double FKArrayMap::FKTransverse(const Point& k) const
{
  return fkPower(k, _transverseCrossSpectra[rotationIndex(k)])/(_stations.count()*_stations.count());
}

double FKArrayMap::FKRayleigh(const Point& k) const
{
  TRACE;
  double fn=1.0/(_stations.count()*_stations.count());
  ComplexMatrix e, eh;
  const ComplexMatrix& covmat=_rayleighCrossSpectra[rotationIndex(k)];
  FKRayleighSteering(k, e, eh, k.z());
  return fkPower(covmat, e, eh)*fn;
}

double FKArrayMap::FKRayleighPositive(const Point& k) const
{
  TRACE;
  ComplexMatrix e, eh;
  const ComplexMatrix& covmat=_rayleighCrossSpectra[rotationIndex(k)];
  FKRayleighSteering(k, e, eh, 1.0);
  return fkPower(covmat, e, eh)/(_stations.count()*_stations.count());
}

double FKArrayMap::FKRayleighNegative(const Point& k) const
{
  TRACE;
  ComplexMatrix e, eh;
  const ComplexMatrix& covmat=_rayleighCrossSpectra[rotationIndex(k)];
  FKRayleighSteering(k, e, eh, -1.0);
  return fkPower(covmat, e, eh)/(_stations.count()*_stations.count());
}

double FKArrayMap::FKEllipticitySign(const Point& k) const
{
  TRACE;
  ComplexMatrix e, eh;
  const ComplexMatrix& covmat=_rayleighCrossSpectra[rotationIndex(k)];
  FKRayleighSteering(k, e, eh, 1.0);
  double valPrograde=fkPower(covmat, e, eh);
  FKRayleighSteering(k, e, eh, -1.0);
  double valRetrograde=fkPower(covmat, e, eh);
  return valPrograde>valRetrograde ? 1.0 : -1.0;
}

double FKArrayMap::HRFKVertical(const Point& k) const
{
  return hrfkPower(k, _verticalCrossSpectrum);
}

double FKArrayMap::HRFKRadial(const Point& k) const
{
  return hrfkPower(k, _radialCrossSpectra[rotationIndex(k)]);
}

double FKArrayMap::HRFKTransverse(const Point& k) const
{
  return hrfkPower(k, _transverseCrossSpectra[rotationIndex(k)]);
}

double FKArrayMap::HRFKRayleigh(const Point& k) const
{
  TRACE;
  ComplexMatrix e, eh;
  HRFKRayleighRadialSteering(k, e, eh, k.z());
  return hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
}

double FKArrayMap::HRFKEllipticity(const Point& k) const
{
  TRACE;
  double maxRadial=-std::numeric_limits<double>::infinity(), maxVertical=-std::numeric_limits<double>::infinity();
  double ellz=1, ellh=1.0;
  ComplexMatrix e, eh;
  double val;
  for(double ell=-2.5; ell<=2.5; ell+=0.05) {
    HRFKRayleighRadialSteering(k, e, eh, ell);
    val=hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
    if(val>maxRadial) {
      ellh=ell;
      maxRadial=val;
    }
    HRFKRayleighVerticalSteering(k, e, eh, ell);
    val=hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
    if(val>maxVertical) {
      ellz=ell;
      maxVertical=val;
    }
  }
  if(ellh<0.0 && ellz<0.0) {
    return -::sqrt(ellz*ellh);
  } else if(ellh>0.0 && ellz>0.0){
    return ::sqrt(ellz*ellh);
  } else {
    return 0.0;
  }
}

double FKArrayMap::HRFKRayleighNoise(const Point& k) const
{
  TRACE;
  double maxRadial=-std::numeric_limits<double>::infinity(), maxVertical=-std::numeric_limits<double>::infinity();
  double ellz=1, ellh=1.0;
  ComplexMatrix e, eh;
  double val;
  for(double ell=-2.5; ell<=2.5; ell+=0.05) {
    HRFKRayleighRadialSteering(k, e, eh, ell);
    val=hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
    if(val>maxRadial) {
      ellh=ell;
      maxRadial=val;
    }
    HRFKRayleighVerticalSteering(k, e, eh, ell);
    val=hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
    if(val>maxVertical) {
      ellz=ell;
      maxVertical=val;
    }
  }
  return _stations.count()*(::sqrt(ellz/ellh)-1.0);
}

double FKArrayMap::HRFKPoggiEllipticity(const Point& k) const
{
  TRACE;
  return ::sqrt(HRFKRadial(k)/HRFKVertical(k));
}

double FKArrayMap::HRFK2Rayleigh(const Point& kell) const
{
  TRACE;
  ComplexMatrix e, eh;
  HRFK2RayleighSteering(kell, e, eh, kell.z());
  double val=hrfkPower(_rayleighCrossSpectra[rotationIndex(kell)], e, eh);
  return val*val*kell.z()*kell.z();
}

double FKArrayMap::HRFK2Ellipticity(const Point& k) const
{
  TRACE;
  double max=-std::numeric_limits<double>::infinity();
  double maxEll=1.0;
  ComplexMatrix e, eh;
  for(double ell=-2.5; ell<=2.5; ell+=0.05) {
    HRFK2RayleighSteering(k, e, eh, ell);
    double val=hrfkPower(_rayleighCrossSpectra[rotationIndex(k)], e, eh);
    val*=val*ell*ell;
    if(val>max) {
      maxEll=ell;
      max=val;
    }
    /*if(k.y()<0.02 && k.y()>-0.02 && k.x()>0.1 && k.x()<0.3) {
      App::log(QString("%1 %2 %3").arg(k.x()).arg(ell).arg(val) << endl;
    }*/
  }
  return maxEll;
}

double FKArrayMap::HRFKDirectLove(const Point& k) const
{
  TRACE;
  ComplexMatrix e, eh;
  HRFKDirectLoveSteering(k, e, eh);
  return hrfkPower(_loveCrossSpectrum, e, eh);
}

double FKArrayMap::HRFKDirectRayleigh(const Point& kell) const
{
  TRACE;
  ComplexMatrix e, eh;
  HRFKDirectRayleighSteering(kell, e, eh, kell.z());
  return hrfkPower(_rayleighCrossSpectrum, e, eh);
}

double FKArrayMap::HRFKDirectEllipticity(const Point& k) const
{
  TRACE;
  double maxRadial=-std::numeric_limits<double>::infinity(), maxVertical=-std::numeric_limits<double>::infinity();
  double ellz=1, ellh=1.0;
  ComplexMatrix e, eh;
  double val;
  for(double ell=-2.5; ell<=2.5; ell+=0.05) {
    HRFKDirectRayleighSteering(k, e, eh, ell);
    val=hrfkPower(_rayleighCrossSpectrum, e, eh);
    if(val>maxRadial) {
      ellh=ell;
      maxRadial=val;
    }
    val*=ell*ell;
    if(val>maxVertical) {
      ellz=ell;
      maxVertical=val;
    }
  }
  double ellRatio=ellz/ellh;
  if(ellRatio<0.0) {
    return 0.0;
  } else {
    double RoverN=0.5*(::sqrt(1+8.0*ellz/ellh)-3.0);
    return ellh*(1.0+RoverN);
  }
}

double FKArrayMap::HRFK2DirectRayleigh(const Point& kell) const
{
  TRACE;
  ComplexMatrix e, eh;
  HRFKDirectRayleighSteering(kell, e, eh, kell.z());
  double val=hrfkPower(_rayleighCrossSpectrum, e, eh);
  return pow(val,1.5)*kell.z()*kell.z();
}

double FKArrayMap::HRFK2DirectEllipticity(const Point& k) const
{
  TRACE;
  double max=-std::numeric_limits<double>::infinity();
  double maxEll=1.0;
  ComplexMatrix e, eh;
  for(double ell=-2.5; ell<=2.5; ell+=0.05) {
    HRFKDirectRayleighSteering(k, e, eh, ell);
    double val=hrfkPower(_rayleighCrossSpectrum, e, eh);
    val=pow(val,1.5)*ell*ell;
    if(val>max) {
      maxEll=ell;
      max=val;
    }
  }
  return maxEll;
}

void FKArrayMap::addSource()
{
  TRACE;
  int nSrc=_sourceSig.count();
  _sourceSig.resize(nSrc+1);
  _sourceSig[nSrc]=new SourceSignal;
}

void FKArrayMap::setSourceCount(int nSrc)
{
  TRACE;
  qDeleteAll(_sourceSig);
  _sourceSig.resize(nSrc);
  for(int i=0; i<nSrc; i++) {
    _sourceSig[i]=new SourceSignal;
  }
}

void FKArrayMap::removeSource(int index)
{
  TRACE;
  int nSrc=_sourceSig.count();
  if(index>=nSrc) return;
  delete _sourceSig[index];
  _sourceSig.remove(index);
}

void FKArrayMap::clearSignals()
{
  TRACE;
  int nStat=_stations.count();
  if(_stationSig) {
    for(int iComp=0; iComp<3; iComp++) {
      for(int iStat=0; iStat<nStat; iStat++) {
        delete [] _stationSig[iComp][iStat];
      }
      delete [] _stationSig[iComp];
    }
    delete [] _stationSig;
  }
}


void FKArrayMap::setStationSignals(double noiseRatio, bool normalize, int blockCount)
{
  TRACE;
  clearSignals();
  int nStat=_stations.count();
  int nSrc=_sourceSig.count();
  if(_sourceSig.isEmpty()) {
    return;
  }
  _stationSig=new Complex**[3];
  for(int iComp=0; iComp<3; iComp++) {
    _stationSig[iComp]=new Complex *[nStat];
    for(int iStat=0; iStat<nStat; iStat++) {
      _stationSig[iComp][iStat]=new Complex[blockCount];
    }
  }
  Angle sr;
  sr.setDegrees(_sensorRotation);
  Complex src, srcX, srcY;

  for(int iSrc=0; iSrc<nSrc; iSrc++) {
    SourceSignal * sig=_sourceSig[iSrc];
    switch(sig->parameters().polarization()) {
    case Mode::Vertical:
      for(int iStat=0; iStat<nStat; iStat++) {
        for(int iBlock=0; iBlock<blockCount; iBlock++) {
          _stationSig[2][iStat][iBlock]+=sig->stationSignal(iBlock, iStat);
        }
      }
      break;
    case Mode::Radial:
      for(int iStat=0; iStat<nStat; iStat++) {
        double a=sig->parameters().azimuthMath(_waveModel, _stations[iStat]);
        for(int iBlock=0; iBlock<blockCount; iBlock++) {
          src=sig->stationSignal(iBlock, iStat);
          srcX=src*cos(a);
          srcY=src*sin(a);
          _stationSig[0][iStat][iBlock]+=srcX*sr.cos()+srcY*sr.sin();
          _stationSig[1][iStat][iBlock]+=srcY*sr.cos()-srcX*sr.sin();
        }
      }
      break;
    case Mode::Rayleigh: {
        Complex ell(0, -sig->parameters().ellipticity());
        for(int iStat=0; iStat<nStat; iStat++) {
          double a=sig->parameters().azimuthMath(_waveModel, _stations[iStat]);
          for(int iBlock=0; iBlock<blockCount; iBlock++) {
            src=sig->stationSignal(iBlock, iStat);
            srcX=src*cos(a);
            srcY=src*sin(a);
            _stationSig[0][iStat][iBlock]+=(srcX*sr.cos()+srcY*sr.sin())*ell;
            _stationSig[1][iStat][iBlock]+=(srcY*sr.cos()-srcX*sr.sin())*ell;
            _stationSig[2][iStat][iBlock]+=src;
          }
        }
      }
      break;
    case Mode::Transverse:
    case Mode::Love:
      for(int iStat=0; iStat<nStat; iStat++) {
        double a=sig->parameters().azimuthMath(_waveModel, _stations[iStat]);
        for(int iBlock=0; iBlock<blockCount; iBlock++) {
          src=sig->stationSignal(iBlock, iStat);
          srcX=src*(-sin(a));
          srcY=src*cos(a);
          _stationSig[0][iStat][iBlock]+=srcX*sr.cos()+srcY*sr.sin();
          _stationSig[1][iStat][iBlock]+=srcY*sr.cos()-srcX*sr.sin();
        }
      }
      break;
    }
  }
  switch(_polarization) {
  case FK_Vertical:
    crossPowerVertical(blockCount, normalize, noiseRatio);
    break;
  case FK_Radial:
    crossPowerRadial(blockCount, normalize, noiseRatio);
    break;
  case FK_Transverse:
    crossPowerTransverse(blockCount, normalize, noiseRatio);
    break;
  case FK_Rayleigh:
  case FK_RayleighPositive:
  case FK_RayleighNegative:
  case FK_EllipticitySign:
    crossPowerRayleigh(blockCount, normalize, noiseRatio);
    break;
  case HRFK_Vertical:
    crossPowerVertical(blockCount, normalize, noiseRatio);
    inverseCrossPower(_verticalCrossSpectrum);
    break;
  case HRFK_Radial:
    crossPowerRadial(blockCount, normalize, noiseRatio);
    inverseCrossPower(_radialCrossSpectra);
    break;
  case HRFK_Transverse:
    crossPowerTransverse(blockCount, normalize, noiseRatio);
    inverseCrossPower(_transverseCrossSpectra);
    break;
  case HRFK_PoggiEllipticity:
    crossPowerVertical(blockCount, normalize, noiseRatio);
    inverseCrossPower(_verticalCrossSpectrum);
    crossPowerRadial(blockCount, normalize, noiseRatio);
    inverseCrossPower(_radialCrossSpectra);
    break;
  case HRFK_Rayleigh:
  case HRFK_Ellipticity:
  case HRFK_RayleighNoise:
  case HRFK2_Rayleigh:
  case HRFK2_Ellipticity:
  case HRFK2_AbsEllipticity:
    crossPowerRayleigh(blockCount, normalize, noiseRatio);
    inverseCrossPower(_rayleighCrossSpectra);
    break;
  case HRFK_DirectLove:
    crossPowerDirectLove(blockCount, normalize, noiseRatio);
    inverseCrossPower(_loveCrossSpectrum);
    break;
  case HRFK_DirectRayleigh:
  case HRFK_DirectAbsEllipticity:
  case HRFK_DirectEllipticity:
  case HRFK2_DirectRayleigh:
  case HRFK2_DirectAbsEllipticity:
  case HRFK2_DirectEllipticity:
    crossPowerDirectRayleigh(blockCount, normalize, noiseRatio);
    inverseCrossPower(_rayleighCrossSpectrum);
    break;
  }

}

int FKArrayMap::rotationIndex(const Point& k) const
{
  double angle=::atan2(k.y(), k.x()); // From -M_PI to M_PI
  if(angle<0.0) {
    angle+=2.0*M_PI;
  }
  // Angle from 0 to 2*M_PI
  int index=qRound(angle/_rotateStep);
  if(index<_rotateStepCount) {
    return index;
  } else {
    return 0;
  }
}
