#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, t1, t2;   /* 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 dummy = 0.;


  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; 

// AM 11/2002: fixed line below: u v are the direction cosines of the 
// *downgoing* photon. Hence, third component must be negative!
// This was a serious bug affecting all versions before 0.6 (see TDAS 
// note on Reflector program 0.6).

  r[2] = (float) -sqrt(1.0 - r[0]*r[0] - r[1]*r[1]);

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

  /* CBC */
  Debug("@0 x r %f %f %f %f %f %f\n", x[0], x[1], x[2], 
	                              r[0], r[1], r[2]);

  /* 
  x[0] = 125.0; 
  x[1] = 125.0; 
  x[2] = 0.0;  */                          /* ground => obs. level =>  z=0   */

  /*  get director cosines x,y on ground */  
  /*
  r[0] = 0.0;     
  r[1] = 0.0;    
  r[2] = -1.0;    
  */
  /* CBC */

  /*!@'

    @#### 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);
          
  /*  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 x r %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 );        

  /* CBC */
  Debug("@2 xCT rCT %f %f %f %f %f %f\n", xCT[0], xCT[1], xCT[2], 
	rCT[0], rCT[1], rCT[2]);
  /* CBC */


  /* 
   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]);

  /*  FIXED Lines below, May 2002, AM : formerly (up to V0.4) 
   *  there was a confusion between telescope coordinates xCT and 
   *  the original coordinates x. Thanks to T. Hengstebeck for 
   *  reporting the bug.
   */

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

  /* Alternative calculation (AM), same result:
   *
   * a = SQR(rCT[0])+SQR(rCT[1]);
   * b = 2*xCT[0]*rCT[0]+2*xCT[1]*rCT[1]-4*ct_Focal_mean*rCT[2];
   * c = -4*ct_Focal_mean*xCT[2]+SQR(xCT[0])+SQR(xCT[1]);
   */

  if ( fabs(a) < 1.e-3 ) {   /* Changed old cut value 1e-6 AM, 04/2003 */
    xcut[2] = -c / b;         // Only one solution

      /*
       * Alternative calculation:
       *  if (a < 1.e-3 )
       *  {
       *     xcut[2] = xCT[2] - c/b*rCT[2];
       */

  } else {

    /* Introduce positiveness check, AM 3/7/2002 */

    dummy = b*b - 4.0*a*c;

    if (dummy < 0.)           /* No intersection */
      return 2;

    d = (float) sqrt(dummy);

    /*  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;

    /*
     * Alternative calculation:
     *
     * xcut[2] = (t1 < t2) ? xCT[2]+t1*rCT[2] : xCT[2]+t2*rCT[2];  
     */
  }
    
  /*
   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]);

  /* CBC */
  Debug("@3 xcut %f %f\n", xcut[0], xcut[1]);
  /* CBC */ 

  /*  convert to Curvilinear distance over the parabolic dish */ 
          
  sx = Lin2Curv( xcut[0] );
  sy = Lin2Curv( xcut[1] );

  /* CBC */
  Debug("@4 sx sy %f %f\n", sx, sy);
  /* CBC */ 

  /*  is it outside the dish? */ 
  if ((fabs(sx) > ct_max_radius) || (fabs(sy) > ct_max_radius)) {
    /* 
    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].sx - sx) +
			     SQR(ct_data[i].sy - sy));

    if (distmirr2 < distmirr) {
      i_mirror = i;
      distmirr  = distmirr2;
    }
  }

  /* 
   the mirror to use is i_mirror (calculated 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;         
  }
            
  /* CBC */
  Debug("@5 theta phi %f %f\n", ct_data[i_mirror].theta, 
	                        ct_data[i_mirror].phi);
  /* CBC */ 

  /*  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 */
        
  /* CBC */
  Debug("@6 mirror %f %f %f\n",ct_data[i_mirror].x,
	                    ct_data[i_mirror].y,
	                    ct_data[i_mirror].z);
  /* CBC */ 

  /* 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;
 

  /* CBC */
  Debug("@7 xmm %f %f %f\n", xmm[0], xmm[1], xmm[2]);
  /* CBC */ 
       
  /* ...then rotation */ 

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

  /* CBC */
  Debug("@8 xm rm %f %f %f %f %f %f\n", xm[0], xm[1], xm[2], 
	                          rm[0], rm[1], rm[2]);
  /* CBC */ 

  /* 
   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;

  /* CBC */
  Debug("@9 rm-norm %f %f %f\n", rm[0], rm[1], rm[2]);
  /* CBC */ 

  /* 
   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]);

  /* CBC */
  Debug("@10 xcut %f %f %f\n", xcut[0], xcut[1], xcut[2]);
  /* CBC */ 

  /* 
  ++
   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:
  */ 

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

  /* CBC */
  Debug("@11 rnor %f %f %f\n", rnor[0], rnor[1], rnor[2]);
  /* CBC */ 

// Changed AM, 11/2002: now we use the normal vector going "outwards" 
// from inside the sphere (=removed minus sign in normalization below).
// It is easier to do so, since now the vector rm indicating the 
// photon direction also goes from the front to the back of the mirror.

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

  /* CBC */
  Debug("@12 rnor-norm %f %f %f\n", rnor[0], rnor[1], rnor[2]);
  /* CBC */ 

  /* 
   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))
  */ 

// AM 11/2002: removed absolute value in scalar 
// product below (it is now unnecessary):

  calpha = (float) (rnor[0]*rm[0] + rnor[1]*rm[1] + rnor[2]*rm[2]);

  /* CBC */
  Debug("@13 calpha %f\n", calpha);
  /* CBC */ 
        
  /*  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]);

  /* CBC */
  Debug("@14 rrefl %f %f %f\n", rrefl[0], rrefl[1], rrefl[2]);
  /* CBC */ 

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

  /* CBC */
  Debug("@15 rrefl-norm %f %f %f\n", rrefl[0], rrefl[1], rrefl[2]);
  /* CBC */ 

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

  /*  first rotation... */

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

  /* CBC */
  Debug("@16 xcutCT rreflCT %f %f %f %f %f %f\n", xcutCT[0], xcutCT[1], 
	xcutCT[2], rreflCT[0], rreflCT[1], rreflCT[2]);
  /* CBC */ 

  /*  ...then translation */ 
 
  xcutCT[0] += ct_data[i_mirror].x;
  xcutCT[1] += ct_data[i_mirror].y;
  xcutCT[2] += ct_data[i_mirror].z;

  /* CBC */
  Debug("@17 xcutCT %f %f %f\n", xcutCT[0], xcutCT[1], xcutCT[2]);
  /* CBC */ 
 
  /*
   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;

  /* CBC */
  Debug("@18 xcam %f %f %f\n", xcam[0], xcam[1], xcam[2]);
  /* CBC */ 
     
  /* 
  ++
   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];

  /* CBC */
  Debug("@19 xcam-AD %f %f \n", xcam[0], xcam[1]);
  /* CBC */ 
 
  /* 
  ++
   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);

  /* CBC */
  Debug("@20 xcam-SM %f %f \n", xcam[0], xcam[1]);
  /* CBC */ 

  /*  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 x + 0 y + z - ct_Focal_mean = 0 => (A,B,C,D) = (0,0,1,-ct_Focal_mean)
   from Table 3.20 "Tasch. der Math."
  */ 
  
  /* AM, 15/11/2002: changed sign to get the angle between photon trajectory
   * and camera plane positive! This had to be changed because now the vector 
   * indicating the reflected photon direction has the opposite sign!
   */

  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!!
  */ 
  /* AM 15/11/2002  Fixed BUG in timing!!! The time to be subtracted 
   * (mirror till ground) had the wrong sign!!!
   */     
  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("@22 %f %f %f\n" 
	"@23 %f %f %f %f %f %f\n"
	"@24 %f %f %d %f %f %f %f\n"
	"@25 %f %f %f %f\n\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 */ 

  /* AM Nov 2002: Added one further change of coordinates so that the camera 
   * images have the "right" orientation: they will appear as seen by an 
   * observer on ground, standing behind the mirror dish and looking towards 
   * the camera. Formerly cph->x and cph->y were simply xcam[0] and xcam[1].
   */

  cph->x   = -xcam[1];
  cph->y   = -xcam[0];

  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);
*/

  double k = 0.25/ct_Focal_mean;
  return ((2*k*x*sqrt(1+4*k*k*x*x)+asinh(2*k*x))/4/k);
}

/*!---------------------------------------------------------------------
// @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);
        
  }

}
