//  Classes to compute 2D 
// R. Ansari - Nov 2008, May 2010 

#include "mdish.h"


//--------------------------------------------------
// -- Four2DResponse class 
//--------------------------------------------------
// Constructor
Four2DResponse::Four2DResponse(int typ, double dx, double dy, double lambda)
  : typ_(typ), dx_((dx>1.e-3)?dx:1.), dy_((dy>1.e-3)?dy:1.)
{
  setLambdaRef(lambda);
  setLambda(lambda);
}

// Return the response for the wave vecteor (kx,ky)
double Four2DResponse::Value(double kx, double ky)
{
  kx *= lambda_ratio_;
  ky *= lambda_ratio_;
  double wk,wkx,wky;
  switch (typ_) 
    {
    case 1:   // Reponse gaussienne parabole diametre D exp[ -(1/8) (lambda  k_g / D )^2 ]
      wk = sqrt(kx*kx+ky*ky)/dx_;
      wk = wk*wk/8.;
      return exp(-wk);
      break;
    case 2:   // Reponse parabole diametre D  Triangle <= kmax= 2 pi D / lambda   
      wk = sqrt(kx*kx+ky*ky)/dx_/2./M_PI;
      return ( (wk<1.)?(1.-wk):0.);
      break;
    case 22:   // Reponse parabole diametre D  Triangle <= kmax= 2 pi D / lambda + trou au centre
      wk = sqrt(kx*kx+ky*ky)/dx_/2./M_PI;
      if (wk<0.025) return 39.*wk;
      else if (wk<1.) return (1.-wk);
      else return 0.;
      break;
    case 3:   // Reponse rectangle Dx x Dy  Triangle (|kx|,|k_y|) <= (2 pi Dx / lambda, 2 pi Dx / lambda) 
      wkx = fabs(kx)/2./M_PI/dx_; 
      wky = fabs(ky)/2./M_PI/dy_; 
      return ( ((wkx<1.)&&(wky<1.))?((1.-wkx)*(1-wky)):0.);
      break;
    default:
      return 1.;
    }
}
// Return a 2 D histrogram as the response(kx,ky)
Histo2D Four2DResponse::GetResponse(int nx, int ny)
{
  double kxmx = 1.2*DeuxPI*dx_;
  double kymx = 1.2*DeuxPI*dy_;
  if (typ_<3) kymx=kxmx; 
  Histo2D h2(-kxmx,kxmx,nx,-kymx,kymx,ny);

  double xbc,ybc;
  for(int_4 j=0; j<h2.NBinY(); j++) 
    for(int_4 i=0; i<h2.NBinX(); i++) {
      h2.BinCenter(i,j,xbc,ybc);
      h2(i,j) = Value(xbc,ybc);
    }
  return h2;	
}

// Return a 2 D histrogram as the response(u=kx/2 pi, v=ky/2 pi)
Histo2D Four2DResponse::GetResponseUV(int nx, int ny)
{
  double kxmx = 1.2*dx_;
  double kymx = 1.2*dy_;
  if (typ_<3) kymx=kxmx; 
  Histo2D h2(-kxmx,kxmx,nx,-kymx,kymx,ny);

  double xbc,ybc;
  for(int_4 j=0; j<h2.NBinY(); j++) 
    for(int_4 i=0; i<h2.NBinX(); i++) {
      h2.BinCenter(i,j,xbc,ybc);
      h2(i,j) = Value(xbc*DeuxPI,ybc*DeuxPI);
    }
  return h2;	
}

HProf Four2DResponse::GetProjNoiseLevel(int nbin, bool fgnorm1)
{
  Histo2D h2w = GetResponse(2*nbin, 2*nbin);
  r_8 vmin=h2w.VMin();
  r_8 vmax=h2w.VMax();
  double seuil=vmax/10000.;
  if (seuil<1.e-6) seuil=1.e-6;
  r_8 facnorm=((fgnorm1)?vmax:1.);
  cout << " Four2DResponse::GetProjNoiseLevel Min,Max=" << vmin << " , " << vmax 
       << " facnorm=" << facnorm << " seuil=" << seuil << endl;  
  double kmax=2.*M_PI*D();
  HProf hp(0,kmax,nbin);
  double x,y;
  for(sa_size_t j=0; j<h2w.NBinY(); j++) {
    for(sa_size_t i=0; i<h2w.NBinX(); i++) {
      h2w.BinCenter(i,j,x,y);
      double yw=h2w(i,j);
      if (yw<seuil) continue;
      hp.Add(sqrt(x*x+y*y),facnorm/yw);
    }
  }
  return hp;
}

HProf Four2DResponse::GetProjResponse(int nbin, bool fgnorm1)
{
  Histo2D h2w = GetResponse(2*nbin, 2*nbin);
  r_8 vmin=h2w.VMin();
  r_8 vmax=h2w.VMax();
  r_8 facnorm=((fgnorm1)?(1./vmax):1.);
  cout << " Four2DResponse::GetProjResponse Min,Max=" << vmin << " , " << vmax 
       << " facnorm=" << facnorm << endl; 
  double kmax=2.*M_PI*D();
  HProf hp(0,kmax,nbin);
  double x,y;
  for(sa_size_t j=0; j<h2w.NBinY(); j++) {
    for(sa_size_t i=0; i<h2w.NBinX(); i++) {
      h2w.BinCenter(i,j,x,y);
      hp.Add(sqrt(x*x+y*y),h2w(i,j)*facnorm);
    }
  }
  return hp;
}

//---------------------------------------------------------------
// -- Four2DRespTable : Reponse tabulee instrumentale ds le plan k_x,k_y (angles theta,phi) 
//---------------------------------------------------------------
Four2DRespTable::Four2DRespTable()
  : Four2DResponse(0,1.,1.)
{
}

Four2DRespTable::Four2DRespTable(Histo2D const & hrep, double d, double lambda)
  : Four2DResponse(0,d,d,lambda) , hrep_(hrep)
{
}

double Four2DRespTable::Value(double kx, double ky)
{
  kx *= lambda_ratio_;
  ky *= lambda_ratio_;
  int_4 i,j;
  if ( (kx<=hrep_.XMin())||(kx>=hrep_.XMax()) || 
       (ky<=hrep_.YMin())||(ky>=hrep_.YMax()) )  return 0.;
  hrep_.FindBin(kx, ky, i, j);
  return hrep_(i, j);
}

double Four2DRespTable::renormalize(double max)
{
  double cmx = hrep_.VMax();
  hrep_ *= (max/cmx);
  return cmx;
}

void Four2DRespTable::writeToPPF(string flnm)
{
  DVList dvinfo;
  dvinfo["DoL"] = dx_;
  dvinfo["LambdaRef"] = lambdaref_;
  dvinfo["Lambda"] = lambda_;
  POutPersist po(flnm);
  po << hrep_;
  po << dvinfo;
}

void Four2DRespTable::readFromPPF(string flnm)
{
  PInPersist pin(flnm);
  DVList dvinfo;
  pin >> hrep_;
  pin >> dvinfo;
  dx_ = dy_ = dvinfo["DoL"];
  setLambdaRef((double)dvinfo["LambdaRef"]);
  setLambda((double)dvinfo["Lambda"]); 
}



//---------------------------------------------------------------
// -- Four2DRespRatio : rapport de la reponse entre deux objets Four2DResponse
//---------------------------------------------------------------
Four2DRespRatio::Four2DRespRatio(Four2DResponse& a, Four2DResponse& b, double maxratio)
  : Four2DResponse(0, a.D(), a.D()), a_(a), b_(b), maxratio_(maxratio), zerothr_(.5/maxratio)
{
}

double Four2DRespRatio::Value(double kx, double ky)
{
  double ra = a_.Value(kx,ky);
  double rb = b_.Value(kx,ky);
  if ((ra<zerothr_)||(rb<zerothr_))  return 0.;
  double rval=ra/rb;
  if (rval<maxratio_) return rval;
  return maxratio_;
}

//---------------------------------------------------------------
//--- Classe simple pour le calcul de rotation 
class Rotation {
public:
  Rotation(double tet, double phi)
  {
// (Teta,Phi) = Direction de visee 
// Les angles d'Euler correspondants sont Teta, Phi+Pi/2
// Le Pi/2 vient que les rotations d'euler se font dans l'ordre
//  Autour de oZ d'angle Phi, autour de oN (nouvel axe X) d'angle Teta
//  Autour du nouvel axe Z (x3) d'angle Psi  (Psi=0 -> cp=1, sp=0.;
  double ct = cos(tet);
  double st = sin(tet);
  // Le Pi/2 echange les axes X<>Y pour theta=phi=0 !
  //  double cf = cos(phi+M_PI/2);
  //  double sf = sin(phi+M_PI/2);
  double cf = cos(phi);
  double sf = sin(phi);
  double cp = 1.; // cos((double)pO);
  double sp = 0.; // sin((double)pO);
  RE[0][0] = cf*cp-sf*ct*sp;     RE[0][1] = sf*cp+cf*ct*sp;      RE[0][2] = st*sp;
  RE[1][0] = -cf*sp-sf*ct*cp;    RE[1][1] = -sf*sp+cf*ct*cp;     RE[1][2] = st*cp;
  RE[2][0] = sf*st;              RE[2][1] = -cf*st;              RE[2][2] = ct;
  }
  inline void Do(double& x, double& y)
  {
    double xx=x; 
    double yy=y;
    x = RE[0][0]*xx+RE[0][1]*yy;
    y = RE[1][0]*xx+RE[1][1]*yy;
  }
  double RE[3][3];
};


//----------------------------------------------------------------------
//  -- Pour calculer la reponse ds le plan kx,ky d'un system MultiDish 
//----------------------------------------------------------------------
MultiDish::MultiDish(double lambda, double dmax, vector<Dish>& dishes, bool fgnoauto)
  : lambda_(lambda), dmax_(dmax), dishes_(dishes), fgnoauto_(fgnoauto)
{ 
  SetThetaPhiRange();
  SetRespHisNBins();
  SetBeamNSamples();
  SetPrtLevel();
  fgcomputedone_=false;
  mcnt_=0;
}

void MultiDish::ComputeResponse()
{
  cout << " MultiDish::ComputeResponse() - NDishes=" << dishes_.size() << " nx=" << nx_ << " ny=" << ny_ << endl;
  double kmx = 1.2*DeuxPI*dmax_/lambda_;
  double dkmx = kmx/(double)nx_;
  double dkmy = kmx/(double)ny_;
  double kmxx = ((double)nx_+0.5)*dkmx;
  double kmxy = ((double)ny_+0.5)*dkmy;
  h2w_.Define(-kmxx,kmxx,2*nx_+1,-kmxy,kmxy,2*ny_+1);
  h2w_.SetZeroBin(0.,0.);
  kmax_=kmx;

  double dold = dishes_[0].Diameter()/lambda_;
  double dolx = dishes_[0].DiameterX()/lambda_;
  double doly = dishes_[0].DiameterY()/lambda_;

  Four2DResponse rd(2, dold, dold);
  Four2DResponse rdr(3, dolx, doly);

  if (!dishes_[0].isCircular())  rd = rdr;

  double dtet = thetamax_/(double)ntet_;
  double dphi = phimax_/(double)nphi_;
  cout << " MultiDish::ComputeResponse() - ThetaMax=" << thetamax_ << " NTheta=" << ntet_ 
       << " PhiMax=" <<  phimax_ << " NPhi=" << nphi_ << endl;

  double sumw = 0.;
  for(int kt=0; kt<ntet_; kt++) {
    double theta=(double)kt*dtet;
    for(int jp=0; jp<nphi_; jp++) {
      double phi=(double)jp*dphi;
      sumw += CumulResp(rd, theta, phi);
      if (theta<1.e-9) continue;
      sumw += CumulResp(rd, theta, -phi);
      sumw += CumulResp(rd, theta, phi+M_PI);
      sumw += CumulResp(rd, theta, -(phi+M_PI));
    }
    if (prtlev_>0) 
      cout << "  MultiDish::ComputeResponse() done ktheta=" << kt << " / MaxNTheta= " 
	   << ntet_ << endl; 
  }
  r_8 rmin,rmax;
  h2w_.GetMinMax(rmin,rmax);
  cout << "  MultiDish::ComputeResponse() finished : Rep_min,max=" << rmin << "," << rmax << " sumW0=" 
       << sumw << " ?=" << h2w_.SumWBinZero() << endl;
  fgcomputedone_=true;
  return;
}

Histo2D MultiDish::GetResponse()
{
  if (!fgcomputedone_) ComputeResponse();

  double kx1 = DeuxPI*(dishes_[0].DiameterX())/lambda_;
  double ky1 = DeuxPI*(dishes_[0].DiameterY())/lambda_;
  int_4 ib,jb;
  //  h2w_ /= h2cnt_; 
  Histo2D h2 = h2w_.Convert();
  h2.FindBin(kx1, ky1, ib, jb);
  if ((kx1<0)||(ky1<0)||(kx1>=h2.NBinX())||(ky1>=h2.NBinY())) {
    cout << " MultiDish::GetResponse[1]/ERROR kx1,ky1=" << kx1 <<","<< ky1 << " --> ib,jb=" << ib <<","<< jb << endl;
    ib=jb=0;
  }
  double sumw=h2w_.SumWBinZero();
  double vmax=h2.VMax();
  cout << " MultiDish::GetResponse[1] VMin=" << h2.VMin() << " VMax= " << vmax  
       << " h(0,0)=" << h2(0,0) << " kx1,ky1->h(" << ib <<"," << jb << ")=" << h2(ib,jb) <<endl;
  //  double fnorm=sqrt((double)dishes_.size())/h2.VMax();
  double fnorm=1.;
  if (vmax > sumw) {
    fnorm=(double)dishes_.size()/h2.VMax();
    cout << " MultiDish::GetResponse[2]/Warning h2.VMax()=" << vmax << " >  sumw=" << sumw << endl;  
    cout << "   ... NDishes=" << dishes_.size() << " sumw=" << sumw 
	 << " Renormalizing x NDishes/VMax  " << fnorm << endl;
  }
  else {
    fnorm=(double)dishes_.size()/sumw;
    cout << " MultiDish::GetResponse[3] NDishes=" << dishes_.size() << " sumw=" << sumw  
	 << " Renormalizing x NDishes/sumw   " << fnorm << endl;
  }
  h2 *= fnorm;
  cout << " ---- MultiDish::GetResponse/[4] APRES VMin=" << h2.VMin() << " VMax= " << h2.VMax() << " h(0,0)=" 
       << h2(0,0) << endl;
  return h2;
}

HProf MultiDish::GetProjNoiseLevel(int nbin, bool fgnorm1)
{
  r_8 vmin,vmax;
  h2w_.GetMinMax(vmin,vmax);
  double seuil=vmax/10000.;
  if (seuil<1.e-6) seuil=1.e-6;
  r_8 facnorm=((fgnorm1)?vmax:1.);
  cout << " MultiDish::GetProjNoiseLevel Min,Max=" << vmin << " , " << vmax 
       << " facnorm=" << facnorm << " seuil=" << seuil << endl;  
  HProf hp(0,kmax_,nbin);
  for(sa_size_t j=0; j<h2w_.NBinY(); j++) {
    double y=h2w_.Y(j);
    for(sa_size_t i=0; i<h2w_.NBinX(); i++) {
      double x=h2w_.X(i);
      double yw=h2w_(i,j);
      if (yw<seuil) continue;
      hp.Add(sqrt(x*x+y*y),facnorm/yw);
    }
  }
  return hp;
}

HProf MultiDish::GetProjResponse(int nbin, bool fgnorm1)
{
  r_8 vmin,vmax;
  h2w_.GetMinMax(vmin,vmax);
  r_8 facnorm=((fgnorm1)?(1./vmax):1.);
  cout << " MultiDish::GetProjResponse Min,Max=" << vmin << " , " << vmax 
       << " facnorm=" << facnorm << endl;  
  HProf hp(0,kmax_,nbin);
  for(sa_size_t j=0; j<h2w_.NBinY(); j++) {
    double y=h2w_.Y(j);
    for(sa_size_t i=0; i<h2w_.NBinX(); i++) {
      double x=h2w_.X(i);
      hp.Add(sqrt(x*x+y*y),h2w_(i,j)*facnorm);
    }
  }
  return hp;
}


Histo2D MultiDish::PosDist(int nx, int ny, double dmax)
{
  if (dmax<1e-3)  dmax=nx*dishes_[0].Diameter();
  double dd = dmax/nx/2.;
  Histo2D hpos(-dd,dmax+dd,nx+1,-dd,dmax+dd,ny+1);
  for(size_t i=0; i<NbDishes(); i++) {
    hpos.Add(dishes_[i].X, dishes_[i].Y);
  }
  return hpos;
}

double MultiDish::AddToHisto(double kx0, double ky0, double x, double y, double w, bool fgfh)
{
  double xxp = kx0+x;
  double yyp = ky0+y;
  double sumw=0.;
  sumw += h2w_.Add(xxp, yyp, w, fgfh);
  double xxm=kx0-x;
  double yym=ky0-y;
  //  if (xxm>0.)  {
  sumw += h2w_.Add(xxm, yyp, w, fgfh);
  // if (yym>0.)  
  sumw += h2w_.Add(xxm, yym, w, fgfh);
  //  }
  // if (yym>0.)  
  sumw += h2w_.Add(xxp, yym, w, fgfh);
  return sumw; 
}

double MultiDish::CumulResp(Four2DResponse& rd, double theta, double phi)
{
  //  cout << " MultiDish::CumulResp()  theta=" << theta << " phi=" << phi << endl;
  /*
  double dx = h2w_.WBinX()/5;
  double dy = h2w_.WBinY()/5;
  int nbx = DeuxPI*rd.Dx()/dx+1;
  int nby = DeuxPI*rd.Dy()/dy+1;
  */
  double dx,dy;
  int nbx=beamnx_; 
  int nby=beamny_;
  dx = DeuxPI*rd.Dx()/(double)nbx;
  dy = DeuxPI*rd.Dy()/(double)nby;
  if (mcnt_==0) 
    cout << " CumulResp() nbx=" << nbx << " nby=" << nby << " dx=" << dx << " dy=" << dy << endl;
  mcnt_++;

  double sumw = 0.;
  Rotation rot(theta, phi);

  for(size_t i=0; i<dishes_.size(); i++) {
    for(size_t j=i; j<dishes_.size(); j++) {
      double kx0 = DeuxPI*(dishes_[i].X-dishes_[j].X)/lambda_;
      double ky0 = DeuxPI*(dishes_[i].Y-dishes_[j].Y)/lambda_;
      double pgain=dishes_[i].Gain()*dishes_[j].Gain();
      rot.Do(kx0, ky0);
      //    if (kx0<0) kx0=-kx0;
      //    if (ky0<0) ky0=-ky0;
      bool fgfh= (!fgnoauto_ || (dishes_[i].ReflectorId()!=dishes_[j].ReflectorId()));
      for(int ix=0; ix<nbx; ix++) 
	for(int jy=0; jy<nby; jy++) { 
	  double x = ix*dx;  
	  double y = jy*dy;
	  if ((ix>0)&&(jy>0)) {
	    sumw += AddToHisto(kx0, ky0, x, y, rd(x,y)*pgain, fgfh);
	    if (j!=i) sumw += AddToHisto(-kx0, -ky0, x, y, rd(x,y)*pgain, fgfh);
	  }
	  else {
	    if ((ix==0)&&(jy==0)) {
	      sumw += h2w_.Add(kx0, ky0, rd(0.,0.)*pgain, fgfh);
	      if (j!=i) sumw += h2w_.Add(-kx0, -ky0, rd(0.,0.)*pgain, fgfh);
	    }
	    else {
	      double w = rd(x,y)*pgain;
	      if (ix==0) {
		sumw += h2w_.Add(kx0, ky0+y, w, fgfh);
		sumw += h2w_.Add(kx0, ky0-y, w, fgfh);
		if (j!=i) {
		  sumw += h2w_.Add(-kx0, -ky0+y, w, fgfh);
		  sumw += h2w_.Add(-kx0, -ky0-y, w, fgfh);
		}
	      }
	      else {
		sumw += h2w_.Add(kx0+x, ky0, w, fgfh);
		sumw += h2w_.Add(kx0-x, ky0, w, fgfh);
		if (j!=i) {
		  sumw += h2w_.Add(-kx0+x, -ky0, w, fgfh);
		  sumw += h2w_.Add(-kx0-x, -ky0, w, fgfh);
		}		
	      }
	    }
	    //	 
	  }
	}
    //    if (i%10==0) 
    //      cout << " MultiDish::CumulResp() done i=" << i << " / imax=" << dishes_.size() 
    //	   << " theta=" << theta << " phi=" << phi << endl;
    }
  }
  return sumw;
}