#include "lobe.h"
#include "radutil.h"
#include "randfmt.h"      
typedef FMTRandGen RandomGenerator ;


#include "fftwserver.h" 
#include "matharr.h"    
#include "ctimer.h"    


/* --Methode-- */
BeamEffect::BeamEffect(Four2DResponse& resp,  bool preservefreq0)
  : fresp_(resp), preservefreq0_(preservefreq0) 
// resp doit avoir sa longueur d'onde de reference en metres
{
}



/* --Methode-- */
void BeamEffect::ApplyLobe3D(TArray< TF >& a, double dx, double dy, double f0, double df)
// dx, dy en radioans, f0, df en MHz
{
  Timer tm("BeamEffect::ApplyLobe3D");
  FFTWServer ffts(true);                     
  ffts.setNormalize(true); 
  
  H21Conversions conv;
  
  TArray< complex<TF> > fourAmp;
  double dkx = DeuxPI/(double)a.SizeX()/dx;
  double dky = DeuxPI/(double)a.SizeY()/dy;
  
  for(sa_size_t kz=0; kz<a.SizeZ(); kz++) {
    TArray< TF > slice( a(Range::all(), Range::all(), kz) );
    ffts.FFTForward(slice, fourAmp);
    conv.setFrequency(f0+kz*df);
    fresp_.setLambda(conv.getLambda());
    //    cout << " DEBUG*" << kz << " lambda=" << conv.getLambda() << " lambda_ratio_=" << fresp_.lambda_ratio_ << endl;
    ApplyLobeK2D(fresp_, fourAmp, dkx, dky);
    ffts.FFTBackward(fourAmp, slice, true);
    if (kz%20==0)  cout << "BeamEffect::ApplyLobe3D() done kz=" << kz << " / a.SizeZ()=" << a.SizeZ() << endl; 
  }
  double mean, sigma;
  TF min, max;
  a.MinMax(min, max);
  MeanSigma(a, mean, sigma);
  cout << " BeamEffect::ApplyLobe3D() - Result Min=" << min << " Max=" << max 
       << " Mean=" << mean << " Sigma=" << sigma << endl; 
  return;
}

/* --Methode-- */
void BeamEffect::Correct2RefLobe(Four2DResponse& rep, TArray< TF >& a, double dx, double dy, double f0, double df)
// dx, dy en radioans, f0, df en MHz
{
  Timer tm("BeamEffect::Correct2RefLobe");
  FFTWServer ffts(true);                     
  ffts.setNormalize(true); 
  
  H21Conversions conv;
  
  TArray< complex<TF> > fourAmp;
  double dkx = DeuxPI/(double)a.SizeX()/dx;
  double dky = DeuxPI/(double)a.SizeY()/dy;
  
  for(sa_size_t kz=0; kz<a.SizeZ(); kz++) {
    TArray< TF > slice( a(Range::all(), Range::all(), kz) );
    ffts.FFTForward(slice, fourAmp);
    conv.setFrequency(f0+kz*df);
    fresp_.setLambda(conv.getLambda());
    Four2DRespRatio rratio(rep, fresp_);
    ApplyLobeK2D(rratio, fourAmp, dkx, dky);
    ffts.FFTBackward(fourAmp, slice, true);
    if (kz%20==0)  cout << "BeamEffect::Correct2RefLobe() done kz=" << kz << " / a.SizeZ()=" << a.SizeZ() << endl; 
  }
  double mean, sigma;
  TF min, max;
  a.MinMax(min, max);
  MeanSigma(a, mean, sigma);
  cout << " BeamEffect::Correct2RefLobe() - Result Min=" << min << " Max=" << max 
       << " Mean=" << mean << " Sigma=" << sigma << endl; 
  return;
}

/* --Methode-- */
void BeamEffect::ApplyLobeK2D(Four2DResponse& rep, TArray< complex<TF> >& fourAmp, double dkx, double dky)
{
  complex<TF> cf0=fourAmp(0,0);
  double kxx, kyy;
  for(sa_size_t ky=0; ky<fourAmp.SizeY(); ky++) {
    kyy =  (ky>fourAmp.SizeY()/2) ? -(double)(fourAmp.SizeY()-ky)*dky : (double)ky*dky; 
    for(sa_size_t kx=0; kx<fourAmp.SizeX(); kx++) {
      kxx=(double)kx*dkx;
      fourAmp(kx, ky) *= complex<TF>(rep(kxx, kyy), 0.);   	
    }
  }
  if (preservefreq0_)  fourAmp(0, 0)=cf0;
  return;
}

/* --Methode-- */
TArray< TF > BeamEffect::ReSample(TArray< TF >& a, double xfac, double yfac, double zfac)
{
  Timer tm("BeamEffect::ReSample");

  sa_size_t szx = a.SizeX()*xfac;
  sa_size_t szy = a.SizeY()*yfac;
  sa_size_t szz = a.SizeZ()*zfac;

  TArray<TF> rsa(szx, szy, szz);
  for(sa_size_t kz=0; kz<rsa.SizeZ(); kz++) {
    sa_size_t kza=kz/zfac;
    if ((kza<0)||(kza>=a.SizeZ()))  continue;
    for(sa_size_t ky=0; ky<rsa.SizeY(); ky++) {
      sa_size_t kya=ky/yfac;
      if ((kya<0)||(kya>=a.SizeY()))  continue;
      for(sa_size_t kx=0; kx<rsa.SizeX(); kx++) {
	sa_size_t kxa=kx/xfac;
	if ((kxa<0)||(kxa>=a.SizeX()))  continue;
	rsa(kx,ky,kz)=a(kxa,kya,kza);
      }
    }
  }
  return rsa;
}


/* --Methode-- */
void BeamEffect::AddNoise(TArray< TF >& a, double pixsignoise, bool fgcmsig)
{
  cout << "BeamEffect::AddNoise() PixelSigmaNoise=" << pixsignoise << endl;
  RandomGenerator rg;
  for(sa_size_t kz=0; kz<a.SizeZ(); kz++) 
    for(sa_size_t ky=0; ky<a.SizeY(); ky++) 
      for(sa_size_t kx=0; kx<a.SizeX(); kx++) 
	a(kx,ky,kz) += rg.Gaussian(pixsignoise);
  if (fgcmsig) {
    double mean, sigma;
    MeanSigma(a, mean, sigma);
    cout << "BeamEffect::AddNoise()-done,  Mean=" << mean << " Sigma=" << sigma << endl;
  }
  return;
}