#include "sopnamsp.h"
#include "FSAppIrrSmpl.h"


FSApproximationIrregularSampling::FSApproximationIrregularSampling()  : fftIntfPtr_(NULL) {;}

FSApproximationIrregularSampling::FSApproximationIrregularSampling(TVector<double>& sampling, double offset, double range) 
  {
    initFFT();
    makeSamplingVector(sampling, offset, range);
    M_ = 0;
  }


FSApproximationIrregularSampling::~FSApproximationIrregularSampling()
{if (fftIntfPtr_!=NULL) delete fftIntfPtr_;}

void FSApproximationIrregularSampling::makeRHS(TVector<complex<double> >& coefSolution)
{
  int k;
  if (exponFourier_.Size() == 0) calculeExponentiellesFourier();
  coefSolution.ReSize(2*M_+1);
  coefSolution = complex<double>(0.,0.);
  int nbEchantillons = samplingValues_.Size();

  // initialisation d'un tableau de travail pour calcul des termes
  // du second membre par recurrence
  TVector<complex<double> > travail(nbEchantillons);
  for (k=0; k < nbEchantillons; k++)
    {
      travail(k) = poids_(k)*signal_(k);
      coefSolution(M_) +=  travail(k);
    }

  // recurrence
  for (k=1; k<=M_; k++)
    {
      int j;
      for (j=0; j < nbEchantillons; j++)
	{
	  travail(j) *= exponFourier_(j);
	  coefSolution(M_+k) += travail(j);
	}
      coefSolution(M_-k) = conj(coefSolution(M_+k));
    }
}

void FSApproximationIrregularSampling::makeToeplitzMatrix(int M)
{
  int j,k;
  M_ = M;
  int nbEchantillons = samplingValues_.Size();
  // matrice de Toeplitz
   if (exponFourier_.Size() == 0) calculeExponentiellesFourier();

  TVector<complex<double> > gamma(2*M_+1);
  gamma = complex<double>(0.,0.);
  // initialisation d'un tableau de travail pour calcul des termes
  // de la matrice par recurrence
  TVector<complex<double> > travail(nbEchantillons);
  travail = poids_;

  for (j=0; j<nbEchantillons; j++) gamma(0) += travail(j);

  // recurrence
  for (k=1; k<=2*M_; k++)
    {

      for (j=0; j<nbEchantillons; j++)
	{
	  travail(j) *= exponFourier_(j);
	  gamma(k) += travail(j);
	}

    }
  
  tptz_.setMatrix(gamma);
}
void FSApproximationIrregularSampling::approximateSignal(int M, const TVector<double>& signal)
{
  int k;
    if (delta_ <= 1./(2.*M) )
      {
	cout << " FSApproximationIrregularSampling : BON ECHANTILLONNAGE " << endl;
      }
    else
      {
	cout << " FSApproximationIrregularSampling : ATTENTION : SIGNAL SOUS-ECHANTILLONNE " << endl;
	cout << " deltaMax (normalise) = " << delta_ << " devrait etre inferieur a " << 1./(2.*M) << endl;
	cout << " ecart max intervient entre echantillon no " << nokdelta_ << " et le suivant, abscisse= " <<  samplingValues_(nokdelta_)*samplingRange_+samplingOffset_ << endl;

      }
    //  PrtTim(" avant toeplitz " );

    if ( M != M_ ) makeToeplitzMatrix(M);

    // PrtTim(" fin toeplitz " );

 
   makeSignalVector(signal);
   //  PrtTim(" fin fabrication signal " );

    // second membre
    
    TVector<complex<double> > coefSolution;
    makeRHS(coefSolution);
    int j;
    // PrtTim(" fin fabrication second membre " );

    int niter =  tptz_.gradientToeplitz(coefSolution);
    // int niter =  tptz_.gradientToeplitzPreconTChang(coefSolution);
    cout << " FSApproximationIrregularSampling::approximateSignal : converged in " << niter << " iterations " << endl;
    coefFourier_.ReSize(M_+1);
    coefFourier_ = coefSolution(Range(M_, 2*M_));
}

// la periode normalisee 1 est divisee en nbInterv intervalles
// les valeurs de la solution sont donnes en 0, 1/n, ..... (n-1)/n 
// le calcul est beaucoup plus rapide si nbInterv est pair (FFT)
void FSApproximationIrregularSampling::restaureRegularlySampledSignal(int nbInterv, TVector<double>& solution) const
{
  cout << " c'est celui que je corrige " << endl;
  int k; 
  if (nbInterv < 2*M_+1) 
      {
	solution.ReSize(nbInterv); 
	double delta = 1./nbInterv;
	for (k=0; k<nbInterv; k++)
	  {
	    double u = k*delta;
	    solution(k) = valeursSerie(u);
	  }
      }
    else
      {
	TVector<complex<double> > TFf(nbInterv);
	TVector<complex<double> > bidon(nbInterv);
	TFf = complex<double>(0.,0.);
	TFf(Range(0,M_)) = coefFourier_;
	for ( k=1; k<= M_; k++)
	  { 
	    TFf(nbInterv-k) = conj(coefFourier_(k));
	  }
	fftIntfPtr_-> FFTBackward(TFf, bidon);
	cout << " taille de bidon "<< bidon.Size() << endl;
	solution.ReSize(nbInterv);
	for (k=0; k< nbInterv; k++) solution(k) = bidon(k).real();
      }
  reshapeSignalInUsersFrame(solution);
}

void FSApproximationIrregularSampling::computeSignalOnASampling(const TVector<double>& abscisses, TVector<double>& solution ) const
{
  int k;
  int n= abscisses.Size();
  if (n<=0) 
    {
      cout << " restaurationEnPoints: vecteurs de points vide " << endl;
      return;
    }
  TVector<double> abscissesLocales;
  abscissesLocales = abscisses;
  matchToSamplingReference(abscissesLocales);
  solution.ReSize(n);
  for (k=0; k<n; k++)
    {
      double u = abscissesLocales(k);
      solution(k) =  valeursSerie(u);
    }
  reshapeSignalInUsersFrame(abscisses, solution);

}

double FSApproximationIrregularSampling::estimationConditionnement() const
{
  double deuxDeltaM = 2.*delta_*M_; 
  double cond = (1.+deuxDeltaM)/(1.-deuxDeltaM);
  cond *= cond;
  return cond;
}

void  FSApproximationIrregularSampling::samplingValues(TVector<double>& sv) const 
{
  int k; 
  int n = samplingValues_.Size();
  sv.ReSize(n);
  for (k=0; k<n;k++)
    {
      sv(k) = samplingOffset_+samplingRange_*samplingValues_(k);
    }

}

// terme constant, puis cos, sin, cos, sin......
void FSApproximationIrregularSampling::coeffCosSin(TVector<double>& coef) const
{
  int j;
  coef.ReSize(2*M_+1);
  coef(0) = coefFourier_(0).real();
  for (j=1; j<M_; j++)
    {
      double aj =  2.*coefFourier_(j).real();
      double bj =  -2.*coefFourier_(j).imag();
      coef(2*(j-1)+1) = aj;
      coef(2*(j-1)+2) = bj;
    }
  coef *= normeSignal_;
}  

// exprime les valeurs d'abscisses, selon la reference locale
void FSApproximationIrregularSampling::matchToSamplingReference(TVector<double>& sampling) const

{
  int k;
  int compteur = sampling.Size();
  double fac = 1./samplingRange_;
  for (k=0; k<compteur; k++)
    {
      sampling(k) = (sampling(k)-samplingOffset_)*fac;
 
    }
  if ( sampling(0) <0. || sampling(compteur-1) >1. )
    {
      cout << " matchToSamplingReference: points hors [0.,1.] " << endl;
      cout << " " << sampling(0) << " " << sampling(compteur-1) << endl;
    }
}

// exprime les valeurs d'echantillonnage entre 0 et 1
void FSApproximationIrregularSampling::resizeSamplingIn_0_1(double offset, double range) 
{
  int k;
  int compteur = samplingValues_.Size();
  samplingOffset_ = offset;
  samplingRange_ = range;
  double fac = 1./samplingRange_;
  for (k=0; k<compteur ;k++)
    {
      samplingValues_(k) = (samplingValues_(k)-samplingOffset_)*fac;
    }
}

void FSApproximationIrregularSampling::computeWeights()
{
  int k;
  int nbEchantillons = samplingValues_.Size();
  nokdelta_ = 0;

  // calcul de l'ecart maximum entre deux temps d'echantillonnage consecutifs
  delta_ = samplingValues_(0)-samplingValues_(nbEchantillons-1)+1;
  for (k=0; k< nbEchantillons-1; k++)
    {
      double diff = samplingValues_(k+1)-samplingValues_(k);
      if ( diff > delta_ )
	{
	  delta_ = diff;
	  nokdelta_ = k;
	}
    }
  // calcul des poids (pour tenir compte de l'irregularite de 
  // l'echantillonnage)
  poids_.ReSize(nbEchantillons);
  poids_(0) = 0.5*(samplingValues_(1)-samplingValues_(nbEchantillons-1) + 1.);
  for (k=1; k< nbEchantillons-1; k++)
    {
      poids_(k) = 0.5*(samplingValues_(k+1)-samplingValues_(k-1));
    }
  poids_(nbEchantillons-1) = 0.5*(samplingValues_(0) +1 - samplingValues_(nbEchantillons-2));
  
}
void FSApproximationIrregularSampling::NormSignal()
{
  int k;
  int nbEchantillons = samplingValues_.Size();
  normeSignal_=0;
  for (k=0; k< nbEchantillons; k++)
    {
      double s = signal_(k); 
      normeSignal_ += s*s*poids_(k);
    }
  normeSignal_=sqrt(normeSignal_);
  double fac = 1./normeSignal_;
  signal_ *= fac;
}



void FSApproximationIrregularSampling::reshapeSignalInUsersFrame(const TVector<double>& abscisses, TVector<double>& resultat) const
{
  if (resultat.Size() <= 0) 
    {
      cout << " reshapeSignalInUsersFrame: vecteur solution VIDE" << endl;
    }
  resultat *= normeSignal_;
}

void FSApproximationIrregularSampling::reshapeSignalInUsersFrame(TVector<double>& resultat) const
{
  if (resultat.Size() <= 0) 
    {
      cout << " reshapeSignalInUsersFrame: vecteur solution VIDE" << endl;
    }
  resultat *= normeSignal_;
}


void FSApproximationIrregularSampling::makeSamplingVector(const TVector<double>& sampling, double offset, double range)
{
  samplingValues_.ReSize(sampling.Size());
  samplingValues_ = sampling;
  resizeSamplingIn_0_1(offset, range);
  computeWeights();
}

void FSApproximationIrregularSampling::makeSignalVector(const TVector<double>& signal)
{
  int n = samplingValues_.Size();
    if (n != signal.Size() )
      {
	cout << " echantillonnage et signal n'ont pas les memes dimensions " << endl;
      }
    signal_ = signal;
    NormSignal();
}





void FSApproximationIrregularSampling::restaureSignal(TVector<double>& solution) const
{
  int k;
  int n= samplingValues_.Size();
  if (n<=0) 
    {
      cout << " restaurationEnPoints: vecteurs de points vide " << endl;
      return;
    }
  solution.ReSize(n);
  for (k=0; k<n; k++)
    {
      double u = samplingValues_(k);
      solution(k) = valeursSerie(u);
    }
  reshapeSignalInUsersFrame(solution);
}