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

#include "mdish.h"


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

// Return the response for the wave vecteor (kx,ky)
double Four2DResponse::Value(double kx, double ky)
{
  double wk,wkx,wky;
  switch (typ_) 
    {
    case 1:   // Reponse gaussienne parabole diametre D exp[ - 0.5 (lambda  k_g / D )^2 ]
      wk = sqrt(kx*kx+ky*ky)/dx_;
      wk = 0.5*wk*wk;
      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 3:   // Reponse rectangle Dx x Dy  Triangle (|kx|,|k_y|) <= (2 pi Dx / lambda, 2 pi Dx / lambda) 
      wkx = kx/2./M_PI/dx_; 
      wky = ky/2./M_PI/dy_; 
      return ( ((wkx<1.)&&(wky<1.))?((1.-wkx)*(1-wky)):0.);
      break;
    default:
      return 1.;
    }
}
// Return a vector representing the power spectrum (for checking) 
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(0.,kxmx,nx,0.,kymx,ny);

  for(int j=0; j<h2.NBinY(); j++) 
    for(int i=0; i<h2.NBinX(); i++) 
      h2(i,j) = Value((i+0.5)*h2.WBinX(), (j+0.5)*h2.WBinY());
  return h2;	
}

//---------------------------------------------------------------
// -- Four2DRespTable : Reponse tabulee instrumentale ds le plan k_x,k_y (angles theta,phi) 
//---------------------------------------------------------------
Four2DRespTable::Four2DRespTable(Histo2D const & hrep, double d)
  : Four2DResponse(0,d,d) , hrep_(hrep)
{
}

double Four2DRespTable::Value(double kx, double ky)
{
  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);
}

//--- 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];
};

// -----------------------------------
// -- Classe ressemblant a un histo 2D 
// -----------------------------------
QHis2D::QHis2D()
  : nx(0),ny(0),xmin(0),xmax(0),ymin(0),ymax(0),sumw0(0.)
{
}
QHis2D::QHis2D(r_8 xMin,r_8 xMax,int_4 nxb,r_8 yMin,r_8 yMax,int_4 nyb)
  : nx(0),ny(0),xmin(0),xmax(0),ymin(0),ymax(0),sumw0(0.)
{
  Define(xMin, xMax, nxb, yMin, yMax, nyb);
}
void QHis2D::Define(r_8 xMin,r_8 xMax,int_4 nxb,r_8 yMin,r_8 yMax,int_4 nyb)
{
  nx=nxb; ny=nyb;
  xmin=xMin;  xmax=xMax;  
  ymin=yMin;  ymax=yMax;  
  dxb=(xmax-xmin)/(double)nx;
  dyb=(ymax-ymin)/(double)ny;
  sa_size_t sz[5];  sz[0]=nx;  sz[1]=ny;
  aw.ReSize(2,sz);
  sumw0=0.;
  return;
}
double QHis2D::Add(r_8 x, r_8 y, r_8 w, bool fgfh)
{
  sa_size_t ix = (sa_size_t)((x-xmin)/dxb);
  sa_size_t jy = (sa_size_t)((y-ymin)/dyb);
  if ((ix<0)||(ix>=nx)||(jy<0)||(jy>=ny))  return 0.;
  double rw = ((ix==0)&&(jy==0)) ? w : 0.;
  sumw0 += rw; 
  if (fgfh) aw(ix,jy) += w;
  return rw;
}
Histo2D QHis2D::Convert()
{
  Histo2D h2(xmin,xmax,nx,ymin,ymax,ny);
  for(int_4 j=0; j<ny; j++) 
    for(int_4 i=0; i<nx; i++)  h2(i,j) = aw(i,j);
  return h2;
}

//----------------------------------------------------------------------
//  -- 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();
  mcnt_=0;
}

Histo2D MultiDish::GetResponse()
{
  cout << " MultiDish::GetResponse() - NDishes=" << dishes_.size() << " nx=" << nx_ << " ny=" << ny_ << endl;
  double kmx = 1.2*DeuxPI*dmax_/lambda_;
  /*
  h2w_= Histo2D(0.,kmx,nx_,0.,kmx,ny_);
  h2cnt_= Histo2D(0.,kmx,nx_,0.,kmx,ny_);
  h2w_.Zero();
  h2cnt_.Zero();
  */
  h2w_.Define(0.,kmx,nx_,0.,kmx,ny_);

  double dold = dishes_[0].D/lambda_;
  double dolx = dishes_[0].Dx/lambda_;
  double doly = dishes_[0].Dy/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)ntet_;

  double sumw = 0.;
  for(int kt=0; kt<ntet_; kt++) 
    for(int jp=0; jp<nphi_; jp++) 
      sumw += CumulResp(rd, (double)kt*dtet, (double)jp*dphi);

  double kx0 = DeuxPI*fabs(dishes_[1].X-dishes_[0].X)/lambda_;
  double ky0 = DeuxPI*fabs(dishes_[1].Y-dishes_[0].Y)/lambda_;
  int_4 ib, jb;
  //  h2w_ /= h2cnt_; 
  Histo2D h2 = h2w_.Convert();
  h2.FindBin(kx0, ky0, ib, jb);
  cout << " ---- MultiDish::GetResponse() VMin=" << h2.VMin() << " VMax= " << h2.VMax() 
       << " h(0,0)=" << h2(0,0) << " h(" << ib <<"," << jb << ")=" << h2(ib,jb) <<endl;
  //  double fnorm=sqrt((double)dishes_.size())/h2.VMax();
  double fnorm=1.;
  if (h2.VMax() > sumw) {
    fnorm=(double)dishes_.size()/h2.VMax();
    cout << " ---- MultiDish::GetResponse() NDishes=" << dishes_.size() << " sumw=" << sumw 
	 << " Renormalizing x NDishes/sumw  " << fnorm << endl;
  }
  else {
    fnorm=(double)dishes_.size()/h2.VMax();
    cout << " ---- MultiDish::GetResponse() NDishes=" << dishes_.size() << " VMax=" << h2.VMax()  
	 << " Renormalizing x NDishes/h2.VMax()   " << fnorm << endl;
  }
  h2 *= fnorm;
  cout << " ---- MultiDish::GetResponse() APRES VMin=" << h2.VMin() << " VMax= " << h2.VMax() << " h(0,0)=" 
       << h2(0,0) << endl;
  return h2;
}

/*
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.;
  int_4 ib, jb;
  h2w_.FindBin(xxp, yyp, ib, jb);
  if ((ib==0)&&(jb==0))  sumw+=w;
  if (fgfh) { 
    h2w_.Add(xxp, yyp, w);
    h2cnt_.Add(xxp, yyp, 1.);
  }
  double xxm=kx0-x;
  double yym=ky0-y;
  if (xxm>0.)  {
    h2w_.FindBin(xxm, yyp, ib, jb);
    if ((ib==0)&&(jb==0))  sumw+=w;
    if (fgfh) {
      h2w_.Add(xxm, yyp, w);
      h2cnt_.Add(xxm, yyp, 1.);
    }
    if (yym>0.) { 
      h2w_.FindBin(xxm, yym, ib, jb);
      if ((ib==0)&&(jb==0))  sumw+=w;
      if (fgfh) { 
	h2w_.Add(xxm, yym, w);
	h2cnt_.Add(xxm, yym, 1.);
      }
    }
  }
  if (yym>0.)  { 
    h2w_.FindBin(xxp, yym, ib, jb);
    if ((ib==0)&&(jb==0))  sumw+=w;
    if (fgfh) {
      h2w_.Add(xxp, yym, w);
      h2cnt_.Add(xxp, yym, 1.);
    }
  }
  return sumw; 
}
*/

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;
  int nby = DeuxPI*rd.Dy()/dy;
  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*fabs(dishes_[i].X-dishes_[j].X)/lambda_;
      double ky0 = DeuxPI*fabs(dishes_[i].Y-dishes_[j].Y)/lambda_;
      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;  
	  sumw += AddToHisto(kx0, ky0, x, y, rd(x,y), fgfh);
	}
    }
    //    if (i%10==0) 
    //      cout << " MultiDish::CumulResp() done i=" << i << " / imax=" << dishes_.size() 
    //	   << " theta=" << theta << " phi=" << phi << endl;
  }
  return sumw;
}