/***************************************************************************
**
**  This file is part of waran.
**
**  waran 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.
**
**  waran 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: 2008-07-08
**  Copyright: 2008-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <ArrayCore.h>

#include "RealTimeArrayProcess.h"
#include "ArrayTask.h"
#include "PhaseShifter.h"
#include "RealTimeStationSignals.h"
#include "RealTimeArrayManager.h"

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

  Full description of class still missing
*/

/*!
  Description of constructor still missing
*/
RealTimeArrayProcess::RealTimeArrayProcess(RealTimeArrayManager * manager, ArrayStations * array)
    : Thread(), _manager(manager)
{
  TRACE;

  // Init processing stations
  _phaseShifter=new PhaseShifter(array);
  _phaseShifter->setMaximumSlowness(0.01);  // Fixed to 100 m/s
  int n=array->count();
  for(int i=0; i<n;i++) {
    _stations.append(new RealTimeStationSignals(array->at(i)));
  }

  _waiting=false;
  _terminate=false;
  _signalsLocked=false;
  _gridSize=0.0;
  _fkGridStep=0.0;
  _hrfkGridStep=0.0;
  start();
}

/*!
  Description of destructor still missing
*/
RealTimeArrayProcess::~RealTimeArrayProcess()
{
  TRACE;
}

void RealTimeArrayProcess::setKmin(double k)
{
  _fkGridStep=0.25*k;
  _hrfkGridStep=0.1*k;
}

void RealTimeArrayProcess::setKmax(double k)
{
  _gridSize=k;
}

void RealTimeArrayProcess::run()
{
#if 0
  _manager->_mutex.lock();
  while(true) {
    while(true) {
      if(_terminate.testAndSetOrdered(true, true)) {
        _manager->_mutex.unlock();
        return;
      }
      // Check for new tasks, if nothing to do, the thread goes in sleep mode
      if(_manager->_tasks.isEmpty()) break;
      ArrayTask * t=_manager->takeTask();
      //checkBlocks(true, false);
      run(t);
      //checkBlocks(true, false);
      delete t;
      _manager->_mutex.lock();
    }
    printf("%s: sleeping, manager %p\n",objectName().toLatin1().data(),_manager);
    _waiting.fetchAndStoreOrdered(true);
    _manager->_event.wait(&_manager->_mutex);
    _waiting.fetchAndStoreOrdered(false);
    printf("%s: waking up, manager %p\n",objectName().toLatin1().data(),_manager);
  }
#endif
}

/*!
  Calculate processed signals (filter, fft, offset removal, taper,...) on time window \a win and lock
  this signals for all stations of the array. If successful, you must call unlockTimeWindow() to release the memory.
  Calling unlockTimeWindow() is not mandatory even between two successive calls.
*/
bool RealTimeArrayProcess::lockTimeWindow(const TimeRange * r)
{
  TRACE;
  // Make sure nothing is currently locked
  unlockTimeWindow();
  RealTimeStationSignals * stat;
  int n=_stations.count();
  for(int i=0; i<n; i++ ) {
    stat=_stations.at(i);
    stat->setProcessed( *r, nullptr);
    if( !stat->lockSamples()) {
      for(i--;i>=0;i--) _stations.at(i)->unlockSamples();
      return false;
    }
  }
  _signalsLocked=true;
  return true;
}

/*!
  Can be called only after a successful lockTimeWindow().
*/
void RealTimeArrayProcess::unlockTimeWindow()
{
  TRACE;
  if(_signalsLocked) {
    QList<RealTimeStationSignals *>::iterator it;
    for(it=_stations.begin();it!=_stations.end();++it) (*it)->unlockSamples();
    _signalsLocked=false;
  }
}

#if 0
void RealTimeArrayProcess::run(ArrayTask * t)
{
  //printf("%s task for %lf Hz between %lf s and %lf s\n",objectName().toLatin1().data(),t->frequency().center(),
  //                                                      t->range().start(), t->range().end());
  lockTimeWindow(&t->range());
  const FrequencyBand& f=t->frequency();
  _phaseShifter->setSquaredOmega(f.squaredOmega());
  ComplexMatrix r=crossSpectrum(0, _bands->at(t->frequencyIndex()));
  fk(r, f);
  hrfk(r, f);
  spac(r, f);
  unlockTimeWindow();
}
#endif

ComplexMatrix RealTimeArrayProcess::crossSpectrum(int component, const GaussianFrequencyBand& f)
{
  int n=_stations.count();
  ComplexMatrix covmat(n);
  covmat.zero(0.0);
  ComplexMatrix sig(1,n);
  for(int iFreq=f.minimumIndex();iFreq <= f.maximumIndex();iFreq++ ) {
    // Fill in the cache with signals
    for(int s=0;s < n;s++ ) {
      RealTimeStationSignals * stat=_stations.at(s);
      Complex c=stat->getSignalSpectrum(component, iFreq) * f.taperValue(iFreq);
      sig.at(0,s)=c;
    }
    covmat+=sig.conjugate().transposed()*sig;
  }
  return covmat;
}

void RealTimeArrayProcess::fk(ComplexMatrix r, const FrequencyBand& f)
{
  if(_gridSize==0 || _fkGridStep==0) return;
  GridSearch grid;
  _phaseShifter->setCrossSpectrum(r);
  grid.setFunction(_phaseShifter);
  grid.setGrid( -_gridSize, _gridSize, _fkGridStep, -_gridSize, _gridSize, _fkGridStep);
  grid.globalMax();
  Point2D origin(0, 0);
  Point2D disp(f.center(), origin.distanceTo(grid.pos())/f.omega());
  _manager->_fkDispersion.add(disp);
  grid.takeFunction();
}

extern "C" int zgesvd_(char *jobu, char *jobvt, int *m, int *n,
                        Complex *a, int *lda, double *s, Complex *u,
                        int *ldu, Complex *vt, int *ldvt, Complex *work,
                        int *lwork, double *rwork, int *info);

void RealTimeArrayProcess::hrfk(ComplexMatrix r, const FrequencyBand& f)
{
#if 0
  if(_gridSize==0 || _fkGridStep==0) return;
  GridSearch grid;
  /*
     Computes the auto-power for normalizing, normalizing factor also include the division by the number
     of frequency samples
  */
  int nStations=_stations.count();
  double * scale=new double[ nStations ];
  for(int row=0;row < _selectedStationCount;row++ ) {
    scale[ row ]=1.0/sqrt(covmat[ row * _selectedStationCount + row ].re());
  }
  /*
     Filling in the lower part of the matrix with conjugates of the upper part and normalizing
  */
  for(int row=0;row < _selectedStationCount;row++ ) {
    for(int col=row;col < _selectedStationCount;col++ ) {
      Complex& upper=covmat[ col * _selectedStationCount + row ];
      Complex& lower=covmat[ row * _selectedStationCount + col ];
      upper *= scale[ row ] * scale[ col ];
      lower=conjugate(upper);
    }
  }
  delete [] scale;
  //printf("Full:\n");
  //printMatrix(covmat, _selectedStationCount, _selectedStationCount);
  // Damping
  if(dampingFactor>0) {
    dampingFactor=1.0-dampingFactor;
    for(int row=0;row < _selectedStationCount;row++ ) {
      for(int col=row;col < _selectedStationCount;col++ ) {
        covmat[ row * _selectedStationCount + col ] *= dampingFactor;
      }
    }
    /*
      Add white noise to avoid singular matrix
    */
    dampingFactor=1.0-dampingFactor;
    for(int el=0; el < _selectedStationCount;el++ ) {
      covmat[ el + _selectedStationCount * el ] += dampingFactor;
    }
  }
  //printf("With noise:\n");
  //printMatrix(covmat, _selectedStationCount, _selectedStationCount);
  /*
     need to allocate the matrices, etc.
     I'll use now code from CLAPACK!
    matrices are allocated in 1 dim.-array!
  */
  doublecomplex *a;   /* copy of covmat */
  doublecomplex *vt;   /* nullspace vectors */
  doublecomplex *u;   /* eigenvectors */
  double *s;   /* eigenvalues */
  doublecomplex *work;    /* work space */
  doublereal *rwork;   /* another workspace */
  char jobu, jobvt;    /* flags what to do in zgesvd_ */
  integer lda, ldu, ldvt;   /* dimensions of SVD problem */
  integer lwork, info;    /* dimension of workspaces */
  integer mm, nn;

  a=new doublecomplex[ nostat2 ];
  u=new doublecomplex[ nostat2 ];
  vt=new doublecomplex[ nostat2 ];
  work=new doublecomplex[ 50 * _selectedStationCount + 1 ];
  rwork=new doublereal[ 5 * _selectedStationCount ];
  s=new double [ _selectedStationCount ];

  /*
     Copy covmat to a
  */
  for(int i=nostat2 - 1;i >= 0;i-- ) {
    doublecomplex& aElement=a[ i ];
    Complex& covmatElement=covmat[ i ];
    aElement.r=covmatElement.re();
    aElement.i=covmatElement.im();
  }
  //printf("a\n");
  //printMatrix(a, _selectedStationCount, _selectedStationCount);
  /*********************************************/
  /* now find the eigenvalues and eigenvectors */
  /* driver ZGESVD already sorts the basis!    */
  /*********************************************/
  // CLAPACK is probalby not thread safe... lock it
  jobu='A';
  jobvt='A';
  lda=ldu=ldvt=mm=nn=_selectedStationCount;
  lwork=50 * _selectedStationCount + 1;
  static Mutex mutex;
  mutex.lock();
  zgesvd_(&jobu, &jobvt, &mm, &nn, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, rwork, &info);
  mutex.unlock();

  /* Output is U, SIGMA and VT

        A=U * SIGMA * VT

     VT being the conjugate-transpose matrix of V

        A^-1=V * SIGMA^-1 * UT

     UT being the conjugate-transpose matrix of U
  */ 
  //printf("u\n");
  //printMatrix(u, _selectedStationCount, _selectedStationCount);
  //printf("s\n");
  //printMatrix(s, 1, _selectedStationCount
  //printf("vt\n");
  //printMatrix(vt, _selectedStationCount, _selectedStationCount);*/

  // Check if matrix has a true inverse (good rank)
  delete [] work;
  delete [] rwork;
  delete [] covmat;
  delete [] a;
  double sRef=1e-12 * s[ 0 ];
  for(int row=0;row < _selectedStationCount;row++ ) {
    if(s[ row ] < sRef) {
      static const char * msg=QT_TR_NOOP( "At %1 Hz, effective rank %2 (%3 frequency samples), "
                                            "singular cross spectral matrix. The effective "
                                            "rank should be at least equal to the number of stations (%4). Either "
                                            "increase the time window lenght or the frequency band width. "
                                            "An empy fk map will be produced." );
      if(log) {
        ( *log) += Message::severityString(Message::Warning)
                  + tr(msg).arg(_frequency).arg(row).arg(_iFreqMax-_iFreqMin+1).arg(_selectedStationCount);
      } else {
        App::log(tr(msg).arg(_frequency).arg(row)
                           .arg(_iFreqMax-_iFreqMin+1).arg(_selectedStationCount) << endl;
      }

      for(int row=0;row < _selectedStationCount;row++ ) {
        for(int col=0;col < _selectedStationCount;col++ ) {
          _Rmatrix[ row * _selectedStationCount + col ]=row==col ? 1 : 0;
        }
      }
      delete [] vt;
      delete [] u;
      delete [] s;
      return;
    }
  }
  // allocate a new temporatry matrix for multiplication of S and U
  Complex * invSUT=new Complex[ nostat2 ];
  for(int row=0;row < _selectedStationCount;row++ ) {
    double si=1./s[ row ];
    for(int col=0;col < _selectedStationCount;col++ ) {
      Complex& invSUTElement=invSUT[ col * _selectedStationCount + row ];
      doublecomplex& uElement=u[ row * _selectedStationCount + col ];
      invSUTElement.set(uElement.r, uElement.i);
      invSUTElement=conjugate(invSUTElement);
      invSUTElement *= si;
    }
  }
  delete [] u;
  delete [] s;
  //printf("invSUT\n");
  //printMatrix(invSUT, _selectedStationCount, _selectedStationCount);
  // proceed with V * invSUT
  for(int row=0;row < _selectedStationCount;row++ ) {
    for(int col=0;col < _selectedStationCount;col++ ) {
      Complex& RElement=_Rmatrix[ row * _selectedStationCount + col ];
      RElement=0;
      for(int k=0;k < _selectedStationCount;k++ ) {
        doublecomplex& vtElement=vt[ row * _selectedStationCount + k ];
        Complex tmp(vtElement.r, vtElement.i);
        tmp=conjugate(tmp);
        tmp *= invSUT[ col * _selectedStationCount + k ];
        RElement += tmp;
      }
    }
  }
  delete [] invSUT;
  delete [] vt;
  
  _phaseShifter->setCrossSpectrum(r);
  grid.setFunction(_phaseShifter);
  grid.setGrid( -_gridSize, _gridSize, _hrfkGridStep, -_gridSize, _gridSize, _hrfkGridStep);
  grid.globalMax();
  Point2D origin(0, 0);
  Point2D disp(f.center(), origin.distanceTo(grid.pos())/f.omega());
  _manager->_hrfkDispersion.add(disp);
  grid.takeFunction();
#endif
}

void RealTimeArrayProcess::spac(ComplexMatrix r, const FrequencyBand& f)
{

}
