#include <stdio.h>
#include <math.h>
#include "diag.h"
#include "init.h"
#include "lagrange.h"
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */

/* random numbers */
#define RandomNumber  ranf()

/* Speed of Light in vacuum, in m/s */
#define Speed_of_Light_vacuum 299792458.0f
#define Speed_of_Light_air (Speed_of_Light_vacuum / 1.000293f)

/* Speed of Light in vacuum, in cm/ns */
#define Speed_of_Light_vacuum_cmns (Speed_of_Light_vacuum / 1.0e7f)
#define Speed_of_Light_air_cmns (Speed_of_Light_air / 1.0e7f)

/*  Macros  */
#define SQR(A)  ((A)*(A))
#define NORM(A) ((float) sqrt((SQR(A[0]))+(SQR(A[1]))+(SQR(A[2]))))

/*  Function declarations */
extern float ranf(void); 
void rnormal(double *r, int n);

/*  Static definitions  */
float OmegaCT[3][3];
float OmegaICT[3][3];
float Omega[3][3];
float OmegaI[3][3];

static double NormalRandomNumbers[500];
/*
   From photons on ground, i.e. observation level,  
   to photons on focal plane, i.e. chamber ! 
      
      Mirror reflectivity 
      Mirror reflection 
      Photon position on chamber  
      Position smearing 
      Timing 

      Returned values: 

         0  OK photon reached the chamber 

	 1  Photon lost due to mirror reflectivity   
	 2  Photon lost because out of mirror  
	 3  Photon lost due to black spot    
	 4  Photon lost because reflected out of chamber

*/

int ph2cph(photon *ph, cphoton *cph)
{ 

  float u, v, w;              /* photon director cosines */ 

  float r[3];                 /* photon trajectory */ 
  float x[3];                 /* position of the photon on ground */ 

  float rCT[3];               /* photon trajectory in the system of the CT */
  float xCT[3];               /* photon position on ground (CT) */ 
  float rm[3];                /* photon trajectory in the system of a mirror */
  float xmm[3];               /* intermediate values */ 
  float xm[3];                /* photon position on ground */ 
  float xcut[3];              /* location of the cut sphere-trajectory */ 
  float xcutCT[3];            /* location of the cut sphere-trajectory (CT) */ 

  float rnor[3], rnorm;       /* normal in that point */ 

  float rrefl[3];             /* reflected vector, from the mirror */ 
  float rreflCT[3];           /* reflected vector, from the CT */ 
  float xcam[3];              /* where the photon hits the camera plane */ 

  float calpha;               /* cos(alpha=angle incident/normal) */ 
  float phi;                  /* angle between photon and camera plane */ 

  float a, b, c, t;           /* intermediate variables */ 

  float d;                    /* minimum distance trajectory-mirror center */ 

  float wl;                   /* photon wavelength */ 
  float reflec;               /* reflectivity for a photon */ 

  float h;                    /* photon production height */ 

  int i, k;                   /* simple counters */ 
  int i_mirror=-1;            /* number of a given mirror */


  float distmirr, distmirr2;  /* distances used in MAGIC reflection routine */
  float sx, sy;
  float t1, t2;

  void makeOmega(float theta, float phi);
  void makeOmegaI(float theta, float phi);
  void applyMxV(float M[3][3], float *V, float *Vp);
  float Lin2Curv(float x); 
 
  /* begin code */

  /* get photon wawelength */ 

  wl = ph->w;

  /*  get position on ground */ 
          
  x[0] = ph->x;
  x[1] = ph->y;
  x[2] = 0.0;                            /* ground => obs. level =>  z=0   */

  /*  get director cosines x,y on ground */  

  r[0] = ph->u; 
  r[1] = ph->v; 
  r[2] = (float) sqrt(1.0 - r[0]*r[0] - r[1]*r[1]);

  /* get photon time and production height */    
                    
  h    = ph->h; 

  /*!@'

    @#### Reflectivity of the mirrors.

    We make a 3rd. order interpolation using Lagrange
    polynomials, in order to calculate the reflectivity of the
    mirror for that wavelength. Further developments will
    include also a incidence-angle dependence (which is not very
    important).

  */

  /* ++
   FILTER: REFLECTIVITY R(lambda)
  -- */ 

  /*  find data point to be used in Lagrange interpolation (-> k) */ 

  FindLagrange(Reflectivity,k,wl,nReflectivity);
          
  /*  if random > reflectivity then goes to the TOP of the loop again */
          
  reflec = Lagrange(Reflectivity,k,wl);

  if ( RandomNumber > reflec ) return 1; 


  /*!@'

  @#### Reflection on mirrors.
  We calculate reflected photon direction

  */

  /* ++
   REFLECTION
  -- */ 

  Debug("@1 %f %f %f %f %f %f\n", x[0], x[1], x[2], r[0], r[1], r[2]);
 
  /*  change to the system of the CT */ 
        
  applyMxV( OmegaCT, x, xCT );
  applyMxV( OmegaCT, r, rCT );        

  /* 
   before moving to the system of the mirror 
   we look whether the photon hits a mirror or not

   calculate the intersection of the trajectory of the photon 
   with the GLOBAL DISH !!!
   we reproduce the calculation of the coefficients of the
   second order polynomial in z (=xCT[2]), made with 
   Mathematica 
  */

  /*
   * In[1]:= parab:=z-(x^2+y^2)/(4F)
   *        par1=parab /. {x->x0+u/w(z-z0),y->y0+v/w(z-z0)}
   * 
   * Out[1]=
   *                  u (z - z0) 2         v (z - z0) 2
   *            (x0 + ----------)  + (y0 + ----------)
   *                      w                    w
   *        z - ---------------------------------------
   *                              4 F
   * 
   * In[2]:= CoefficientList[ExpandAll[par1*4F*w^2],z]
   * 
   * Out[2]=
   *            2   2     2   2
   *        {-(w  x0 ) - w  y0  + 2 u w x0 z0 + 
   *         
   *                         2   2    2   2
   *          2 v w y0 z0 - u  z0  - v  z0 , 
   *         
   *              2                            2
   *         4 F w  - 2 u w x0 - 2 v w y0 + 2 u  z0 + 
   *         
   *             2       2    2
   *          2 v  z0, -u  - v } 
   */       
 
  /*  the z coordinate is calculated */
 
  a = - SQR(rCT[0]) - SQR(rCT[1]);

  b = (float) (4.0*ct_Focal_mean*SQR(rCT[2]) 
    - 2.0*rCT[0]*rCT[2]*xCT[0] - 2.0*rCT[1]*rCT[2]*xCT[1] 
    + 2.0*SQR(rCT[0])*xCT[2] + 2.0*SQR(rCT[1])*xCT[2]);

  c = 2*rCT[0]*rCT[2]*x[0]*x[2] + 2*rCT[1]*rCT[2]*x[1]*x[2] 
    - SQR(rCT[2])*SQR(x[0]) - SQR(rCT[2])*SQR(x[1])
    - SQR(rCT[0])*SQR(x[2]) - SQR(rCT[1])*SQR(x[2]);

  if ( fabs(a) < 1.e-6 ) {

    /*  only one value */ 

    xcut[2] = -c / b;

  } else {
          
    d = (float) sqrt( b*b - 4.0*a*c );
          
    /*  two possible values for z */ 
          
    t1 = (float) ((-b+d) / (2.0*a));
    t2 = (float) ((-b-d) / (2.0*a));

    /*  z must be the minimum of t1 and t2 */ 
          
    xcut[2] = (t1 < t2) ? t1 : t2;

  }
    
  /*
   xcut[] is NOW the cut between the GLOBAL dish of MAGIC and
   the trajectory of the photon
  */

  xcut[0] = xCT[0] + rCT[0]/rCT[2]*(xcut[2]-xCT[2]);
  xcut[1] = xCT[1] + rCT[1]/rCT[2]*(xcut[2]-xCT[2]);

  /*  convert to Curvilinear distance over the parabolic dish */ 
          
  sx = Lin2Curv( xcut[0] );
  sy = Lin2Curv( xcut[1] );
          
  /*  is it outside the dish? */ 
          
  if ((fabs(sx) > 850.0) || (fabs(sy) > 850.0)) {
    /* 
    cout << "CONDITION 1 !" << endl << flush;
    cout << '1';
    */
    return 2;
  }

  /*  calculate the mirror to be used */ 

  distmirr = 1000000.0f;
        
  for (i=0; i<ct_NMirrors && distmirr>=ct_RMirror; ++i) {
    distmirr2 = (float) sqrt(SQR(ct_data[i].x - xcut[0]) +
		     SQR(ct_data[i].y - xcut[1]) +
		     SQR(ct_data[i].z - xcut[2]));
    if (distmirr2 < distmirr) {
      i_mirror = i;
      distmirr = distmirr2;
    }
  }

  /* 
   the mirror to use is i_mirror (calculated several lines above)
   check whether the photon is outside the nearest (this) mirror
  */

  if ((fabs(ct_data[i_mirror].sx - sx) > ct_RMirror) ||
      (fabs(ct_data[i_mirror].sy - sy) > ct_RMirror)) {
    /* 
    cout << "CONDITION 2 !" << endl << flush;
    cout << '2';
    */ 
    return 2;         
  }
            
  /*  calculate matrices for the mirror */ 
        
  makeOmega (-ct_data[i_mirror].theta, 
	     ct_data[i_mirror].phi);
  makeOmegaI(-ct_data[i_mirror].theta, 
	     ct_data[i_mirror].phi);
        
  /* change to the system of the mirror */
        
  /* first translation... */

  xmm[0] = xCT[0] - ct_data[i_mirror].x;
  xmm[1] = xCT[1] - ct_data[i_mirror].y;
  xmm[2] = xCT[2] - ct_data[i_mirror].z;
        
  /* ...then rotation */ 

  applyMxV( Omega, xmm, xm );
  applyMxV( Omega, rCT, rm );

  /* 
   the vector rCT should be normalized, and
   so the vector rm remains normalized as well, but, anyhow...
  */

  rnorm  = NORM( rm );
  rm[0] /= rnorm;
  rm[1] /= rnorm;
  rm[2] /= rnorm;

  /* 
   calculate the intersection of the trajectory of the photon 
   with the mirror
   we reproduce the calculation of the coefficients of the
   second order polynomial in z (=xm[2]), made with 
   Mathematica
  */ 

  /*
   * In[1]:= esfera:=x^2+y^2+(z-R)^2-R^2;
   *         recta:={x->x0+u/w(z-z0),y->y0+v/w(z-z0)}
   * 
   * In[2]:= esfera
   * 
   *            2    2    2           2
   * Out[2]= -R  + x  + y  + (-R + z)
   * 
   * In[3]:= recta
   * 
   *                     u (z - z0)            v (z - z0)
   * Out[3]= {x -> x0 + ----------, y -> y0 + ----------}
   *                         w                     w
   * 
   * In[4]:= esf=esfera /. recta
   * 
   *           2           2         u (z - z0) 2         v (z - z0) 2
   * Out[4]= -R  + (-R + z)  + (x0 + ----------)  + (y0 + ----------)
   *                                      w                    w
   * 
   * In[5]:= coefs=CoefficientList[ExpandAll[esf],z]
   * 
   *                                               2   2    2   2
   *            2     2   2 u x0 z0   2 v y0 z0   u  z0    v  z0
   * Out[5]= {x0  + y0  - --------- - --------- + ------ + ------, 
   *                           w           w          2        2
   *                                                 w        w
   *  
   *                                  2         2          2    2
   *             2 u x0   2 v y0   2 u  z0   2 v  z0      u    v
   * >    -2 R + ------ + ------ - ------- - -------, 1 + -- + --}
   *               w        w         2         2          2    2
   *                                 w         w          w    w
   * In[6]:= Simplify[ExpandAll[coefs*w^2]]
   * 
   *           2    2     2                             2    2    2
   * Out[6]= {w  (x0  + y0 ) - 2 w (u x0 + v y0) z0 + (u  + v ) z0 ,
   *  
   *             2             2                            2    2    2
   * >    -2 (R w  - u w x0 + u  z0 + v (-(w y0) + v z0)), u  + v  + w }
   * 
   */
     
  /* 
   the z coordinate is calculated, using the coefficient
   shown above
  */ 
  
  a = SQR(rm[0]) + SQR(rm[1]) + SQR(rm[2]);

  b = (float) (-2*(2.*ct_data[i_mirror].f*SQR(rm[2]) 
	  - rm[0]*rm[2]*xm[0] 
	  + SQR(rm[0])*xm[2] 
	  + rm[1]*(-(rm[2]*xm[1]) + rm[1]*xm[2])));

  c = (SQR(rm[2])*(SQR(xm[0]) + SQR(xm[1])) 
       - 2*rm[2]*(rm[0]*xm[0] + rm[1]*xm[1])*xm[2]  
       + (SQR(rm[0]) + SQR(rm[1]))*SQR(xm[2]));

  d = (float) sqrt( b*b - 4.0*a*c );

  /*  two possible values for z */ 

  t1 = (float) ((-b+d) / (2.0*a));
  t2 = (float) ((-b-d) / (2.0*a));

  /*  z must be the minimum of t1 and t2 */ 

  xcut[2] = (t1 < t2) ? t1 : t2;
  xcut[0] = xm[0] + rm[0]/rm[2]*(xcut[2]-xm[2]);
  xcut[1] = xm[1] + rm[1]/rm[2]*(xcut[2]-xm[2]);

  /* 
  ++
   BLACK SPOTS: If the photon hits the black spot, it's lost
  --
  */ 

  if ( sqrt(SQR(xcut[0]) + SQR(xcut[1])) < ct_BlackSpot_rad ) {
    /* 
    cout << "CONDITION 3!\n" << flush;
    cout << '3';
    */
    return 3;
  }

  /* 
   if we still have the photon, we continue with the reflexion;        
   we calculate normal vector in this point 
   (and normalize, with the sign changed)
  */ 

  rnor[0] = 2.0f*xcut[0];
  rnor[1] = 2.0f*xcut[1];
  rnor[2] = (float) (2.0*(xcut[2] - 2.0*ct_Focal[i_mirror]));

  rnorm    = -NORM( rnor );
  rnor[0] /= rnorm;
  rnor[1] /= rnorm;
  rnor[2] /= rnorm;

  /* 
   now, both "normal" vector and original trajectory are
   normalized
   just project the original vector in the normal, and 
   take it as the "mean" position of the original and
   the "reflected" vector
   from this, we can calculate the "reflected" vector
   calpha = cos(angle(rnor,rm))
  */ 

  calpha = (float) fabs(rnor[0]*rm[0] + rnor[1]*rm[1] + rnor[2]*rm[2]);
        
  /*  finally!!! we have the reflected trajectory of the photon */ 

  rrefl[0] = (float) (2.0*rnor[0]*calpha - rm[0]);
  rrefl[1] = (float) (2.0*rnor[1]*calpha - rm[1]);
  rrefl[2] = (float) (2.0*rnor[2]*calpha - rm[2]);

  rnorm     = NORM( rrefl );
  rrefl[0] /= rnorm;
  rrefl[1] /= rnorm;
  rrefl[2] /= rnorm;

  /*  let's go back to the coordinate system of the CT */ 

  /*  first rotation... */

  applyMxV( OmegaI, xcut, xcutCT);
  applyMxV( OmegaI, rrefl, rreflCT);

  /*  ...then translation */ 
 
  xcutCT[0] += ct_data[i_mirror].x;
  xcutCT[1] += ct_data[i_mirror].y;
  xcutCT[2] += ct_data[i_mirror].z;
 
  /*
   calculate intersection of this trajectory and the camera plane
   in the system of the CT, this plane is z = ct_Focal
  */ 

  t = (ct_Focal_mean - xcutCT[2]) / rreflCT[2];

  xcam[0] = xcutCT[0] + rreflCT[0]*t;
  xcam[1] = xcutCT[1] + rreflCT[1]*t;
  xcam[2] = xcutCT[2] + rreflCT[2]*t;
     
  /* 
  ++
   AXIS DEVIATION: We introduce it here just as a first order 
     correction, by modifying the position of the reflected photon.
  --
  */ 

  xcam[0] += AxisDeviation[0][i_mirror];
  xcam[1] += AxisDeviation[1][i_mirror];

  /* 
  ++
   SMEARING: We apply the point spread function for the mirrors
  --
  */ 

  /*  get two N(0;1) random numbers */ 

  rnormal( NormalRandomNumbers, 2 );

  /*  modify the Cphoton position in the camera */ 

  xcam[0] += (float) (NormalRandomNumbers[0] * ct_PSpread_mean);
  xcam[1] += (float) (NormalRandomNumbers[1] * ct_PSpread_mean);

  /*  check whether the photon goes out of the camera */ 

  if ( (SQR(xcam[0])+SQR(xcam[1])) > SQR(ct_CameraWidth) ) {
    return 4;
  }

  /* 
  ++
   ANGLE OF INCIDENCE
  --

   calculate angle of incidence between tray. and camera plane
   the camera plane is
   0 y + 0 y + z - ct_Focal = 0 => (A,B,C,D) = (0,0,1,-ct_Focal)
   from Table 3.20 "Tasch. der Math."
  */ 
  
  phi = (float) asin(rreflCT[2]);  

  /* 
  ++
   TIMING
  --
  */ 
  
  /*  calculate the new time of the photon (in the camera) */

  t = ph->t;

  /* 
   substract path from the mirror till the ground, 'cos 
   the photon actually hit the mirror!!
  */ 
  
  t = (float) (t + ((( xm[2] > 0. ) ? -1.0 : +1.0) *
	      sqrt( SQR(xm[0] - xcut[0]) +
		    SQR(xm[1] - xcut[1]) +
		    SQR(xm[2] - xcut[2]) ) / Speed_of_Light_air_cmns));
                 
  /*  add path from the mirror till the camera */ 

  t = (float) (t + sqrt( SQR(xcutCT[0] - xcam[0]) +
		SQR(xcutCT[1] - xcam[1]) +
		SQR(xcutCT[2] - xcam[2]) ) / Speed_of_Light_air_cmns);
                  
  /*  show it */ 

  Debug("@2 %f %f %f %f %f %f %f %f %f\n"
	"@3 %f %f %d %f %f %f %f\n"
	"@4 %f %f %f %f\n",
	xCT[0], xCT[1], xCT[2], rCT[0], rCT[1], rCT[2],
	    xcut[0], xcut[1], xcut[2],
	sx, sy, i_mirror, ct_data[i_mirror].sx, ct_data[i_mirror].sy,
	    ct_data[i_mirror].sx - sx, ct_data[i_mirror].sy - sy,
	xcam[0], xcam[1], xcam[2], phi);

  /* Output */ 

/*  cph->w   = wl;  */
  cph->x   = xcam[0];
  cph->y   = xcam[1];
  cph->u   = r[0];
  cph->v   = r[1];
  cph->t   = t;
  cph->h   = h;
  cph->phi = phi; 

  return 0;   

}   /*  end of ph2cph  */


/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 

!---------------------------------------------------------------------
 @name makeOmega                                        
                                                    
 @desc function to calculate the matrix Omega(theta,phi)

 @var    theta   Angle theta of the transformation 
 @var    phi     Angle phi of the transformation 

 @date Sat Jun 27 05:58:56 MET DST 1998
----------------------------------------------------------------------
 @function
*/ 
 
void
makeOmega (float theta, float phi)
{
  static float ct, st, cp, sp;
  
  /*  shortcuts for cosine and sine of theta and phi */
  ct = (float) cos(theta);
  st = (float) sin(theta);
  cp = (float) cos(phi);
  sp = (float) sin(phi);
  
  /*  save values in the array (see top of file) */ 
  Omega[0][0] =  cp*ct;
  Omega[0][1] =  sp*ct; 
  Omega[0][2] = -st; 
     
  Omega[1][0] = -sp;
  Omega[1][1] =  cp;
  Omega[1][2] =  0;      

  Omega[2][0] =  cp*st;
  Omega[2][1] =  sp*st; 
  Omega[2][2] =  ct;         
}


/*
!---------------------------------------------------------------------
 @name makeOmegaI                                         
                                                      
 @desc function to calculate the matrix Omega-1(theta,phi)

 @var    theta   Angle theta of the transformation 
 @var    phi     Angle phi of the transformation 

 @date Sat Jun 27 05:58:56 MET DST 1998
----------------------------------------------------------------------
 @function
*/
 
void
makeOmegaI(float theta, float phi)
{
  static float ct, st, cp, sp;
  
  /*  shortcuts for cosine and sine of theta and phi */ 
  ct = (float) cos(theta);
  st = (float) sin(theta);
  cp = (float) cos(phi);
  sp = (float) sin(phi);
  
  /*  save values in the array (see top of file) */ 
  OmegaI[0][0] =  cp*ct;
  OmegaI[0][1] = -sp;
  OmegaI[0][2] =  cp*st;     

  OmegaI[1][0] =  sp*ct;
  OmegaI[1][1] =  cp; 
  OmegaI[1][2] =  sp*st;

  OmegaI[2][0] = -st;   
  OmegaI[2][1] =  0; 
  OmegaI[2][2] =  ct;            
}


/*
!---------------------------------------------------------------------
 @name applyMxv                                              
                                                         
 @desc returns the vector v' such that v' = M x v

 @var    M       matrix of the transformation
 @var    v       vector to be multiplied
 @var    vi      resulting vector

 @date Sat Jun 27 05:58:56 MET DST 1998
----------------------------------------------------------------------
 @function
*/
 
void
applyMxV(float M[3][3], float *V, float *Vp)
{
  Vp[0] = (M[0][0] * V[0] +
           M[0][1] * V[1] +
           M[0][2] * V[2]);
  Vp[1] = (M[1][0] * V[0] +
           M[1][1] * V[1] +
           M[1][2] * V[2]);
  Vp[2] = (M[2][0] * V[0] +
           M[2][1] * V[1] +
           M[2][2] * V[2]);
}

/*
!---------------------------------------------------------------------
 @name Lin2Curv                        
                                             
 @desc Linear (Euclidean) to Curvilinear distance

 @var x      Radial distance from the axis of the paraboloid

 @return     Curvilinear distance over the parabolic shape

 @date Wed Jul  8 15:25:39 MET DST 1998
----------------------------------------------------------------------
 @function
*/
 
float 
Lin2Curv(float x)
{
  x /= 100.f;
  return ((x + 0.000144175317185f * x * x * x)*100.f);
}

/*!---------------------------------------------------------------------
// @name rnormal                                   
//                                             
// @desc returns n(=2k) normaly distributed numbers
//
// @var *r   pointer to a vector where we write the numbers
// @var  n   how many numbers do we generate
//
// @date Sat Jun 27 05:58:56 MET DST 1998
//----------------------------------------------------------------------
// @function */

void rnormal(double *r, int n)
{

  double z1, z2;
  int i;

  for (i=0; i<n; i+=2) {

    z1 = RandomNumber;
    z2 = RandomNumber;
  
    r[i]   = sqrt(-2.0*log(z1)) * cos(2.0*M_PI*z2);
    r[i+1] = sqrt(-2.0*log(z1)) * sin(2.0*M_PI*z2);
        
  }

}
