#include "mlobe.h"
#include "unitvec.h"

#include "timing.h"
// #include "nbrandom.h"

/* --Methode-- */
MainLobe::MainLobe(float sig, float dels, int nc)
{
if (sig < 1.e-9)  sig = M_PI/100.;   // Sigma du lobe gaussien (radians)
mSigma = sig;
mDels = (dels < 1.e-5) ?  0.5 : dels;   // Epaisseur de chaque couche en unite de sigma
mNC =  (nc < 1) ? 1 : nc;
mNpix = 1;    // Nb total de pixels du lobe

int i,j,k;
for(i=1; i<=mNC; i++)  mNpix += 6*i;
//  mT0 , mP0  Teta et Phi des pixels, Pixel 0 oriente vers (0, 0)
mT0 = new float[mNpix];
mP0 = new float[mNpix];
//  mTC , mPC  Teta et Phi des pixels, Pixel 0 oriente vers (Teta, phi)
mTC = new float[mNpix];
mPC = new float[mNpix];
mDx = new float[mNpix];
mDy = new float[mNpix];
mDr = new float[mNpix];
// Valeurs des pixels 
mVal = new double[mNpix];


double teta,phi,dphi;
double val;
mT0[0] = mP0[0] = mTC[0] = mPC[0] = 0.;
mVal[0] = 1.;  mDx[0] = mDy[0] = 0.;
k = 1;  mTotVal = 1.;
for(i=1; i<=mNC; i++) {
  teta = (double)i*mDels;
  val = exp(-teta*teta);
  teta *= mSigma;
  if (i%2)  phi = M_PI/6.;
  else phi = 0.;
  dphi = M_PI/(3.*i);
  for(j=0; j<6*i; j++) {
    mT0[k] = mTC[k] = teta;
    phi += dphi;
    mP0[k] = mPC[k] = phi;
    mVal[k] = val;
    mTotVal += val;
    mDx[k] = teta*cos(phi);
    mDy[k] = teta*sin(phi);
    mDr[k] = teta;
    k++;
    }
  }
mDirTet = mDirPhi = 0.;
return;
}


/* --Methode-- */
MainLobe::~MainLobe()
{
delete mT0;
delete mP0;
delete mTC;
delete mPC;
delete mDx;
delete mDy;
delete mDr;
delete mVal;
}

/* --Methode-- */
void MainLobe::SetDirection(float teta, float phi)
{
int k;

mDirTet = teta;
mDirPhi = phi;

if (teta < 0.1*mDels*mSigma) {
  for(k=0; k<mNpix; k++) {
    mTC[k] = mT0[k]; mPC[k] = mP0[k]; 
    }
  return;
  }


/*
if (teta > 10.*mSigma*mDels*mNC) {   // On utilise des formules approchees
  float stet = sin((double)teta);
  float ctet = cos((double)teta);
  for(k=0; k<mNpix; k++) {
    mTC[k] = teta - mDy[k]*stet - mDr[k]*ctet;
    mPC[k] = phi + mDx[k];
    }
  return;
  }
  */

// Formules compliquees 

double dx,dy,dz;
double sphi = sin((double)phi);
double cphi = cos((double)phi);
double stet = sin((double)teta);
double ctet = cos((double)teta);

UnitVector pd((double)teta, (double)phi);
UnitVector vc;
for(k=0; k<mNpix; k++) {
  dz = mDy[k]*stet;
  dx = -(mDx[k]*sphi+mDy[k]*ctet*cphi);
  dy = mDx[k]*cphi-mDy[k]*ctet*sphi;
  vc.setxyz(pd.x+dx, pd.y+dy, pd.z+dz);
  mTC[k] = vc.Teta();
  mPC[k] = vc.Phi();
  if (mTC[k] < 0.)  printf("ERREUR !!!! mTC[%d] = %g \n", k, mTC[k]);
  if (mPC[k] < 0.) mPC[k] += M_PI*2.;
  if (mPC[k] >= M_PI*2.) mPC[k] -= M_PI*2.;
  }
return;  
}

/* --Methode-- */
double MainLobe::Value(int kpx, float& teta, float& phi)
{
if ( (kpx < 0) || (kpx >= mNpix) ) { teta = phi = 0.; return(0.); }
teta = mTC[kpx];
phi = mPC[kpx];
return(mVal[kpx]);
}

/* --Methode-- */
double MainLobe::Convol(SphericalMap& sph)
{
double ret=0.;
int k;
for(k=0; k<mNpix; k++)  ret += mVal[k]*sph(mTC[k], mPC[k]);
/*
if( (dbg > 0) && (k%dbg) ) 
  printf("MainLobe::Convol k= %d T,P= %g %g , Lobe=%g Sph= %g  Ret=%g\n", 
         k, mTC[k], mPC[k], mVal[k], sph(mTC[k], mPC[k]), ret );
*/
return(ret/mTotVal);
}

/* --Methode-- */
ostream& operator << (ostream& s, MainLobe const& lob)
{
s << "MainLobe_Info() : Sigma= " << lob.mSigma << " Dels= " << lob.mDels << " NC= " 
  << lob.mNC << " NTotPix= " << lob.mNpix << " Integ= " << lob.mTotVal << endl;

int i,j,k;
cout << " Direction Teta,Phi = " << lob.mDirTet << " , " << lob.mDirPhi << endl;      
cout << " Pixel 0 , Teta= " << lob.mT0[0] << " Phi= " << lob.mP0[0] 
     << " Val= " << lob.mVal[0] << endl;
k = 1;
for(i=1; i<lob.mNC; i++) {
  cout << " Couche # " << i << " NPix= " << i*6 << endl;
  for(j=0;j<6*i;j++) {
    cout << "K= " << k << "Teta,Phi= "  << lob.mTC[k] << ", " << lob.mPC[k] 
         << " (" << lob.mT0[k] << ", " << lob.mP0[k] << ") "
         << " Val= " << lob.mVal[k] << endl; 
    k++;
    }
  }


return s;
}