Index: trunk/Mars/msimreflector/MFresnelLens.cc
===================================================================
--- trunk/Mars/msimreflector/MFresnelLens.cc	(revision 19629)
+++ trunk/Mars/msimreflector/MFresnelLens.cc	(revision 19632)
@@ -18,5 +18,5 @@
 !   Author(s): Thomas Bretz,  6/2019 <mailto:tbretz@physik.rwth-aachen.de>
 !
-!   Copyright: CheObs Software Development, 2000-2009
+!   Copyright: CheObs Software Development, 2000-2019
 !
 !
@@ -26,4 +26,7 @@
 //
 //  MFresnelLens
+//
+//  For some details on definitions please refer to
+//  https://application.wiley-vch.de/berlin/journals/op/07-04/OP0704_S52_S55.pdf
 //
 //////////////////////////////////////////////////////////////////////////////
@@ -33,5 +36,8 @@
 #include <errno.h>
 
+#include <TRandom.h>
+
 #include "MQuaternion.h"
+#include "MReflection.h"
 
 #include "MLog.h"
@@ -52,4 +58,71 @@
 
     fMaxR = 55/2.;
+}
+
+// --------------------------------------------------------------------------
+//
+// Refractive Index of PMMA, according to
+// https://refractiveindex.info/?shelf=organic&book=poly(methyl_methacrylate)&page=Szczurowski
+//
+// n^2-1=\frac{0.99654 l^2}{l^2-0.00787}+\frac{0.18964 l^2}{l^2-0.02191}+\frac{0.00411 l^2}{l^2-3.85727}
+//
+// Returns the refractive index n as a function of wavelength (in nanometers)
+//
+double MFresnelLens::RefractiveIndex(double lambda)
+{
+    const double l2 = lambda*lambda;
+
+    const double c0 = 0.99654/(1-0.00787e6/l2);
+    const double c1 = 0.18964/(1-0.02191e6/l2);
+    const double c2 = 0.00411/(1-3.85727e6/l2);
+
+    return sqrt(1+c0+c1+c2);
+}
+
+// --------------------------------------------------------------------------
+//
+// Ideal angle of the Fresnel surfaces at a distance r from the center
+// to achieve a focal distance F for a positive Fresnel lens made
+// from a material with a refractive index n.
+// A positive Fresnel lens is one which focuses light from infinity
+// (the side with the grooves) to a point (the flat side of the lens).
+//
+// The calculation follows
+// https://shodhganga.inflibnet.ac.in/bitstream/10603/131007/13/09_chapter%202.pdf
+// Here, a thin lens is assumed
+//
+// The HAWC's Eye lens is an Orafol SC943
+// https://www.orafol.com/en/europe/products/optic-solutions/productlines#pl1
+//
+// sin(omega) = r / sqrt(r^2+F^2)
+// tan(alpha) = sin(omega) / [ 1 - sqrt(n^2-sin(omega)^2) ]
+//
+// Return alpha [rad] as a function of the radial distance r, the
+// focal length F and the refractive index n. r and F have to have
+// the same units. The Slope angle is defined with respect to the plane
+// of the lens. (0 at the center, decreasing with increasing radial
+// distance)
+//
+double MFresnelLens::SlopeAngle(double r, double F, double n)
+{
+    double so = r / sqrt(r*r + F*F);
+    return atan(so / (1-sqrt(n*n - so*so))); // alpha<0, Range [0deg; -50deg]
+}
+
+
+//
+// Draft angle of the Orafol SC943 According to the thesis of Eichler
+// and NiggemannTim Niggemann:
+//
+// The surface of the lens follows the shape of a parabolic lens to compensate spherical aberration
+// Draft angle: psi(r) = 3deg + r * 0.0473deg/mm
+//
+// The draft angle is returned in radians and is defined w.r.t. to the
+// normal of the lens surface. (almost 90deg at the center,
+// decreasing with increasing radial distance)
+//
+double MFresnelLens::DraftAngle(double r)
+{
+    return (3 + r*0.473)*TMath::DegToRad(); // Range [0deg; 15deg]
 }
 
@@ -97,4 +170,74 @@
 double CalcIntersection(const MQuaternion &p, const MQuaternion &u, const Groove &g, const double &fThickness)
 {
+/*
+    Z: position of peak
+
+    r_cone(z) = (Z-z)*tan(theta)
+
+    (x)   (p.x)          (u.x)
+    (y) = (p.y)  + dz *  (u.y)
+    (z)   (p.z)          (u.z)
+
+    r_line(z) = sqrt(x^2 + y^2) = sqrt( (p.x+dz*u.x)^2 + (p.y+dz*u.y)^2)
+
+    Solved with wmmaxima:
+
+    solve((H-z)^2*t^2 = (px+(z-pz)/uz*ux)^2+(py+(z-pz)/uz*uy)^2, z);
+
+     [
+      z=-(uz*sqrt((py^2+px^2)*t^2*uz^2+((2*H*py-2*py*pz)*t^2*uy+(2*H*px-2*px*pz)*t^2*ux)*uz+((pz^2-2*H*pz+H^2)*t^2-px^2)*uy^2+2*px*py*ux*uy+((pz^2-2*H*pz+H^2)*t^2-py^2)*ux^2)-H*t^2*uz^2+(-py*uy-px*ux)*uz+pz*uy^2+pz*ux^2)/(t^2*uz^2-uy^2-ux^2)
+      z= (uz*sqrt((py^2+px^2)*t^2*uz^2+((2*H*py-2*py*pz)*t^2*uy+(2*H*px-2*px*pz)*t^2*ux)*uz+((pz^2-2*H*pz+H^2)*t^2-px^2)*uy^2+2*px*py*ux*uy+((pz^2-2*H*pz+H^2)*t^2-py^2)*ux^2)+H*t^2*uz^2+( py*uy+px*ux)*uz-pz*uy^2-pz*ux^2)/(t^2*uz^2-uy^2-ux^2)
+     ]
+*/
+
+    const double Ux = u.X()/u.Z();
+    const double Uy = u.Y()/u.Z();
+
+    const double px = p.X();
+    const double py = p.Y();
+    const double pz = p.Z();
+
+    const double H   = g.fPeakZ;
+
+    const double t   = g.fTanTheta;
+    const double t2  = t*t;
+
+    const double Ur2 = Ux*Ux + Uy*Uy;
+    const double pr2 = px*px + py*py;
+    const double Up2 = Ux*px + Uy*py;
+
+    const double cr2 = Ux*py - Uy*px;
+
+    const double a   = t2 - Ur2;
+    const double b   = Ur2*pz - Up2 - H*t2;
+
+    const double h   = H-pz;
+    const double h2  = h*h;
+
+    // [ -b +-sqrt(b^2 - 4 ac) ] / [ 2a ]
+
+    const double radix = (Ur2*h2 + 2*Up2*h + pr2)*t2 - cr2*cr2;
+
+    if (radix<0)
+        return 0;
+
+    double dz[2] =
+    {
+        (-b+sqrt(radix))/a,
+        (-b-sqrt(radix))/a
+    };
+
+     if (dz[0]<0 && dz[0]>fThickness)
+        return dz[0];
+
+    if (dz[1]<0 && dz[1]>fThickness)
+        return dz[1];
+
+    return 0;
+}
+
+/*
+double CalcIntersection(const MQuaternion &p, const MQuaternion &u, const Groove &g, const double &fThickness)
+{
    // p is the position  in the plane  of the lens (px, py, 0,  t)
    // u is the direction of the photon             (ux, uy, dz, dt)
@@ -103,5 +246,5 @@
 
    // Radius at distance z+dz from the tip and at distance r from the axis of the cone
-   // r = (z+dz) * tan(theta)
+   // r = (z+dz*u.z) * tan(theta)
 
    // Defining
@@ -116,5 +259,5 @@
    // (X)        (u.x)   (p.x)
    // (Y) = dz * (u.y) + (p.y)
-   // (Z)        (u.z)   ( 0 )
+   // (Z)        (u.z)   (p.z)
 
    // Equality
@@ -151,8 +294,10 @@
         return 0;
 
+    const int sign = g.fPeakZ>=0 ? 1 : -1;
+
     const double dz[2] =
     {
-        (sqrtBac+B) / a,
-        (sqrtBac-B) / a
+        (sqrtBac+B) / a * sign,
+        (sqrtBac-B) / a * sign
     };
 
@@ -167,54 +312,5 @@
     return 0;
 }
-
-double CalcRefractiveIndex(double lambda)
-{
-    // https://refractiveindex.info/?shelf=organic&book=poly(methyl_methacrylate)&page=Szczurowski
-
-    // n^2-1=\frac{0.99654λ^2}{λ^2-0.00787}+\frac{0.18964λ^2}{λ^2-0.02191}+\frac{0.00411λ^2}{λ^2-3.85727}
-
-    const double l2 = lambda*lambda;
-
-    const double c0 = 0.99654/(1-0.00787e6/l2);
-    const double c1 = 0.18964/(1-0.02191e6/l2);
-    const double c2 = 0.00411/(1-3.85727e6/l2);
-
-    return sqrt(1+c0+c1+c2);
-}
-
-bool ApplyRefraction(MQuaternion &u, const TVector3 &n, const double &n1, const double &n2)
-{
-    // From: https://stackoverflow.com/questions/29758545/how-to-find-refraction-vector-from-incoming-vector-and-surface-normal
-    // Note that as a default u is supposed to be normalized such that z=1!
-
-    // u: incoming direction (normalized!)
-    // n: normal vector of surface (pointing back into the old medium?)
-    // n1: refractive index of old medium
-    // n2: refractive index of new medium
-
-    // V_refraction = r*V_incedence + (rc - sqrt(1- r^2 (1-c^2)))n
-    // r = n1/n2
-    // c = -n dot V_incedence.
-
-    // The vector should be normalized already
-    // u.NormalizeVector();
-
-    const double r = n2/n1;
-    const double c = n*u.fVectorPart;
-
-    const double rc = r*c;
-
-    const double R = 1 - r*r + rc*rc; // = 1 - (r*r*(1-c*c));
-
-    // What is this? Total reflection?
-    if (R<0)
-        return false;
-
-    const double v = rc + sqrt(R);
-
-    u.fVectorPart = r*u.fVectorPart - v*n;
-
-    return true;
-}
+*/
 
 Int_t MFresnelLens::ExecuteOptics(MQuaternion &p, MQuaternion &u, const Short_t &wavelength) const
@@ -226,102 +322,118 @@
 
     const double R = D/2;    // [cm] radius of lens
-    const double w = 0.0100; // [cm] Width of a single groove
+    const double w = 0.01;   // [cm] Width of a single groove
     const double H = 0.25;   // [cm] Thickness (from pdf)
 
     // Focal length is given at this wavelength
-    const double n0 = CalcRefractiveIndex(546);
-
-    const double n = CalcRefractiveIndex(wavelength==0 ? 546 : wavelength);
+    const double n0 = RefractiveIndex(546);
+    const double n  = RefractiveIndex(wavelength==0 ? 546 : wavelength);
+
+    const double r = p.R();
 
     // Ray has missed the lens
-    if (p.R()>R)
+    if (r>R)
         return -1;
 
-    const int nth = TMath::FloorNint(p.R()/w); // Ray will hit the nth groove
-
-    const double r0 =  nth     *w; // Lower radius of nth groove
-    const double r1 = (nth+1)  *w; // Upper radius of nth groove
-    const double rc = (nth+0.5)*w; // Center of nth groove
-
-    // FIXME: Do we have to check the nth-1 and nth+1 groove as well?
-
-    // Angle of grooves
-    // https://shodhganga.inflibnet.ac.in/bitstream/10603/131007/13/09_chapter%202.pdf
-
-    // This might be a better reference (Figure 1 Variant 1)
-    // https://link.springer.com/article/10.3103/S0003701X13010064
-
-    // I suggest we just do the calculation ourselves (but we need to
-    // find out how our lens looks like)
-    // Not 100% sure, but I think we have SC943
-    // https://www.orafol.com/en/europe/products/optic-solutions/productlines#pl1
-
-    // From the web-page:
-    // Positive Fresnel Lenses ... are usually corrected for spherical aberration.
-
-    // sin(omega) = R / sqrt(R^2+f^2)
-    // tan(alpha) = sin(omega) / [ 1 - sqrt(n^2-sin(omega)^2) ]
-
-    const double so    = rc / sqrt(rc*rc + F*F);
-    const double alpha = atan(so / (1-sqrt(n0*n0 - so*so))); // alpha<0, Range [0deg; -50deg]
-
-    // Tim Niggemann:
-    // The surface of the lens follows the shape of a parabolic lens to compensate spherical aberration
-    // Draft angle: psi(r) = 3deg + r * 0.0473deg/mm
-
-    const double psi = (3 + r0*4.73e-3)*TMath::DegToRad(); // Range [0deg; 15deg]
+    // Calculate the ordinal number of the groove which is hit
+    const unsigned int nth = TMath::FloorNint(r/w);
+
+    // Calculate its lower and upper radius and its center
+    const double r0 = nth*w;       // Lower radius of nth groove
+    const double rc = nth*w + w/2; // Upper radius of nth groove
+    const double r1 = nth*w + w;   // Center of nth groove
+
+    // Calculate the slope and draft angles
+    const double alpha = -SlopeAngle(rc, F, n0);
+    const double psi   =  DraftAngle(r1);
+
+    // Define the corresponding surfaces
+    const Groove slope(TMath::Pi()/2-alpha,  r0*tan(alpha));
+    const Groove draft(-psi,                -r1/tan(psi));
+
+    // Calculate the insersection of the ray between the two surfaces
+    // with z between 0 and -H
+    const double dz_slope = CalcIntersection(p, u, slope, -H);
+    const double dz_draft = CalcIntersection(p, u, draft, -H);
+
+    // valid means that the photon has hit the fresnel surface,
+    // otherwise the draft surface
+    bool valid = dz_slope>dz_draft || dz_draft>=0;
+
+    // Theta angle of the normal vector of the surface that was hit
+    TVector3 norm;
+
+    // Propagate to the hit point. To get the correct normal vector of
+    // the surface, the hit point must be knwone
+    p.PropagateZ(u, valid ? dz_slope : dz_draft);
+    norm.SetMagThetaPhi(1, valid ? alpha : psi-TMath::Pi()/2, p.fVectorPart.Phi());
+
+    // Estimate reflectivity for this particular hit (n1<n2 => check before)
+    // Probability that the ray gets reflected
+    if (gRandom->Uniform()<SchlickReflectivity(u.fVectorPart, norm, 1, n))
+    {
+        // Reflect at the surface
+        u *= MReflection(norm);
+
+        // Upgoing rays are lost
+        if (u.Z()>0)
+            return valid ? -3 : -4;
+
+        // Propgate back to z=0 (FIXME: CalcIntersection requires z=0)
+        p.PropagateZ(u, 0);
+
+        // Calc new intersection the other of the two surfaces
+        valid = !valid;
+
+        const double dz = CalcIntersection(p, u, valid ? slope : draft, -H);
+
+        // Propagate to the hit point at z=dz (dz<0) and get the new normal vector
+        p.PropagateZ(u, dz);
+        norm.SetMagThetaPhi(1, valid ? alpha : psi-TMath::Pi()/2, p.fVectorPart.Phi());
+    }
+
+    const double check_r = p.R();
+
+    // Some sanity checks (we loose a few permille due to numerical uncertainties)
+    // If the ray has not hit within the right radii.. reject
+    if (check_r<r0)
+        return valid ? -5 : -6;
+
+    if (check_r>r1)
+        return valid ? -7 : -8;
 
     // Find dw value of common z-value
     //
-    // tan(90-psi)  =  z/dw
-    // tan(alpha)   = -z/(w-dw)
+    // gz*tan(psi) = w - gw
+    // gz = dw*tan(alpha)
     //
-    // -> dw = w/(1-tan(90-psi)/tan(alpha))
-
-    // In a simplified world, all photons which hit the draft surface get lost
-    // FIXME: This needs proper calculation in the same manner than for the main surface
-    const double dw = w/(1-tan(TMath::Pi()/2-psi)/tan(alpha));
-    if (p.R()>r1-dw)
-        return -1;
-
-    //             theta                   peak_z
-    const Groove g(TMath::Pi()/2 + alpha, -r0*tan(alpha));
-
-    // Calculate the insersection between the ray and the cone and z between 0 and -H
-    const double dz = CalcIntersection(p, u, g, -H);
-
-    // No groove was hit
-    if (dz>=0)
-        return -1;
-
-    // Propagate to the hit point at z=dz (dz<0)
-    p.PropagateZ(u, dz);
-
-    // Check where the ray has hit
-    // If the ray has not hit within the right radius.. reject
-    if (p.R()<=r0 || p.R()>r1)
-        return -1;
-
-    // Normal vector of the surface at the hit point
-    // FIXME: Surface roughness?
-    TVector3 norm;;
-    norm.SetMagThetaPhi(1, alpha, p.XYvector().Phi());
-
-    // Apply refraction at lens entrance (change directional vector)
+    const double Gw =  w/(tan(alpha)*tan(psi)+1);
+    const double Gz = -Gw*tan(alpha);
+
+    if (valid && check_r>r0+Gw)
+        return -9;
+    if (!valid && check_r<r0+Gw)
+        return -10;
+
+    // Apply refraction at the lens entrance surface (changes directional vector)
     // FIXME: Surace roughness
     if (!ApplyRefraction(u, norm, 1, n))
-        return -1;
+        return valid ? -11 : -12;
 
     // Propagate ray until bottom of lens
-    const double v = u.fRealPart; // c changes in the medium 
-    u.fRealPart = n*v;
     p.PropagateZ(u, -H);
-    u.fRealPart = v;  // Set back to c
-
-    // Apply refraction at lens exit (change directional vector)
-    // Normal surface (bottom of lens)
+
+    // The lens exit surface is a plane in X/Y at Z=-H
+    norm = TVector3(0, 0, 1);
+
+    // Apply refraction at the lens exit surface (changes directional vector)
     // FIXME: Surace roughness
-    if (!ApplyRefraction(u, TVector3(0, 0, 1), n, 1))
-        return -1;
+    if (!ApplyRefraction(u, norm, n, 1))
+        return valid ? -13 : -14;
+
+    // Estimate reflectivity for this particular hit (n1>n2 => check after)
+    // Probability that the ray gets reflected
+    // (total internal reflection -> we won't track that further)
+    if (gRandom->Uniform()<SchlickReflectivity(u.fVectorPart, norm, n, 1))
+        return valid ? -15 : -16;
 
     // To make this consistent with a mirror system,
@@ -331,6 +443,7 @@
 
     // In the datasheet, it looks as if F is calculated
-    // towards the center of the lens.
-    p.fVectorPart.SetZ(-H/2);
+    // towards the center of the lens
+    // (Propagating to F means not propagating a distance of F-H/2)
+    p.fVectorPart.SetZ(H/2);
 
     return nth; // Keep track of groove index
Index: trunk/Mars/msimreflector/MFresnelLens.h
===================================================================
--- trunk/Mars/msimreflector/MFresnelLens.h	(revision 19629)
+++ trunk/Mars/msimreflector/MFresnelLens.h	(revision 19632)
@@ -6,4 +6,5 @@
 #endif
 
+class TVector3;
 class MQuaternion;
 
@@ -27,4 +28,10 @@
     Bool_t IsValid() const { return kTRUE; }
 
+    // -----------------------------------------------------------
+
+    static double RefractiveIndex(double lambda);
+    static double SlopeAngle(double r, double F, double n);
+    static double DraftAngle(double r);
+
     ClassDef(MFresnelLens, 1) // Parameter container storing the description of a lens
 };
