//  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.)
{
  ixb0 = jyb0 = 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);
  SetZeroBin();
  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==ixb0)&&(jy==jyb0)) ? w : 0.;
  sumw0 += rw; 
  if (fgfh) aw(ix,jy) += w;
  return rw;
}
void QHis2D::SetZeroBin(r_8 x, r_8 y)
{
  ixb0 = (sa_size_t)((x-xmin)/dxb);
  jyb0 = (sa_size_t)((y-ymin)/dyb);
}
Histo2D QHis2D::Convert()
{
  int_4 imn,jmn,imx,jmx;
  r_8 min = aw(0,0);
  r_8 max = aw(0,0);
  imn=jmn=imx=jmx=0;
  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);
      if (aw(i,j)>max) {
	imx=i;  jmx=j;  max=aw(i,j);
      }
      if (aw(i,j)<min) {
	imn=i;  jmn=j;  min=aw(i,j);
      }
    }
  cout << "QHis2D::Convert()/Info: Nx,Ny=" << nx << "," << ny << " SumW=" << sumw0 
       << "\n ... Max:" << imx << "," << jmx << " ->" << max 
       << " @" << imx*dxb+xmin << "," << jmx*dyb+ymin
       << "\n ...Min:" << imn << "," << jmn << " ->" << min 
       << " @" << imn*dxb+xmin << "," << jmn*dyb+ymin << endl;
  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_;
  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.);

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

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;
  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=0; j<dishes_.size(); j++) {
      double kx0 = DeuxPI*(dishes_[i].X-dishes_[j].X)/lambda_;
      double ky0 = DeuxPI*(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;
	  if ((ix>0)&&(jy>0)) {
	    sumw += AddToHisto(kx0, ky0, x, y, rd(x,y), fgfh);
	  }
	  else {
	    if ((ix==0)&&(jy==0)) 
	      sumw += h2w_.Add(kx0, ky0, rd(0.,0.), fgfh);
	    else {
	      double w = rd(x,y);
	      if (ix==0) {
		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 (i%10==0) 
    //      cout << " MultiDish::CumulResp() done i=" << i << " / imax=" << dishes_.size() 
    //	   << " theta=" << theta << " phi=" << phi << endl;
    }
  }
  return sumw;
}