/* ======================================================================== *\
!
! *
! * This file is part of CheObs, the Modular Analysis and Reconstruction
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appears in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!
!   Author(s): Thomas Bretz,  6/2019 <mailto:tbretz@physik.rwth-aachen.de>
!
!   Copyright: CheObs Software Development, 2000-2019
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
//  MFresnelLens
//
// For some details on definitions please refer to
// https://application.wiley-vch.de/berlin/journals/op/07-04/OP0704_S52_S55.pdf
//
// The HAWC's Eye lens is an Orafol SC943
// https://www.orafol.com/en/europe/products/optic-solutions/productlines#pl1
//
// A good description on ray-tracing can be found here
// https://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
//
//////////////////////////////////////////////////////////////////////////////
#include "MFresnelLens.h"

#include <fstream>
#include <errno.h>

#include <TRandom.h>

#include "MQuaternion.h"
#include "MReflection.h"

#include "MMath.h"

#include "MLog.h"
#include "MLogManip.h"

ClassImp(MFresnelLens);

using namespace std;

// ==========================================================================

enum exception_t
{
    kValidRay = 0,

    kStrayUpgoing,
    kOutsideRadius,
    kNoSurfaceFound,
    kStrayDowngoing,
    kAbsorbed,

    kFoundSurfaceUnavailable,

    kInvalidOrigin,
    kTransitionError,

    kEnter = 1000,
    kLeave = 2000,
};

enum surface_t
{
    kPhotonHasLeft = 0,

    kEntrySurface,
    kSlopeSurface,
    kDraftSurface,
    kExitSurface,

    kMaterial = 5,

    kNoSurface = 9
};


class raytrace_exception : public runtime_error
{
protected:
    int fError;
    int fOrigin;
    int fSurface;

public:
    raytrace_exception(const int &_id, const int &_origin, const int &_surface, const string& what_arg) :
        runtime_error(what_arg), fError(_id), fOrigin(_origin), fSurface(_surface)
    {
    }

    raytrace_exception(const int &_id, const int &_origin, const int &_surface, const char* what_arg) :
        runtime_error(what_arg), fError(_id), fOrigin(_origin), fSurface(_surface)
    {
    }

    int id() const { return fError + fSurface*10 + fOrigin*100; }
    int error() const { return fError; }
    int origin() const { return fOrigin; }
    int surface() const { return fSurface; }
};

class raytrace_error : public raytrace_exception
{
public:
    raytrace_error(const int &_id, const int &_origin, const int &_surface, const string& what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
    raytrace_error(const int &_id, const int &_origin, const int &_surface, const char* what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
};
class raytrace_info : public raytrace_exception
{
public:
    raytrace_info(const int &_id, const int &_origin, const int &_surface, const string& what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
    raytrace_info(const int &_id, const int &_origin, const int &_surface, const char* what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
};
class raytrace_user : public raytrace_exception
{
public:
    raytrace_user(const int &_id, const int &_origin, const int &_surface, const string& what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
    raytrace_user(const int &_id, const int &_origin, const int &_surface, const char* what_arg) :
        raytrace_exception(_id, _origin, _surface, what_arg) { }
};

// ==========================================================================


// --------------------------------------------------------------------------
//
// Default constructor
//
MFresnelLens::MFresnelLens(const char *name, const char *title) :
    fPSF(0), fSlopeAbsorption(false), fDraftAbsorption(false),
    fBottomReflection(true), fDisableMultiEntry(false), fFresnelReflection(true),
    fMinHits(0), fMaxHits(0)
{
    fName  = name  ? name  : "MFresnelLens";
    fTitle = title ? title : "Parameter container storing a collection of several mirrors (reflector)";

    // Default: Orafol SC943

    DefineLens();
}

// ==========================================================================

// --------------------------------------------------------------------------
//
// Default ORAFOL SC943
//
// Focal Length:   F = 50.21 cm
// Diameter:       D = 54.92 cm
// Groove width:   w =  0.01 cm
// Lens thickness: h =  0.25 cm
//
// Default wavelength: 546 nm
//
void MFresnelLens::DefineLens(double F, double D, double w, double h, double lambda)
{
    fR = D/2;  // [cm] Lens radius
    fW = w;    // [cm] Width of a single groove
    fH = h;    // [cm] Thickness of lens
    fF = F;    // [cm] focal length (see also MGeomCamFAMOUS!)

    fLambda = lambda;

    fN = MFresnelLens::RefractiveIndex(fLambda);  // Lens

    // Velocity of light within the lens material [cm/ns]
    // FIXME: Note that for the correct conversion in Transmission()
    // also the speed in the surrounding medium has to be taken correctly
    // into account (here it is assumed to be air with N=1
    fVc = fN/(TMath::C()*100/1e9); // cm/ns

    InitGeometry(fR, fW, fN, fF, fH);
}

// --------------------------------------------------------------------------
//
// Precalculate values such as the intersection points inside the grooves,
// the angle of the slope and draft surface and the corresponding tangents.
//
void MFresnelLens::InitGeometry(double maxr, double width, double N0, double F, double d)
{
    const uint32_t num = TMath::CeilNint(maxr/width);

    fGrooves.resize(num);

    for (uint32_t i=0; i<num; i++)
    {
        const double r0 = i*width;
        const double rc = i*width + width/2;
        const double r1 = i*width + width;

        // Slope angle of the reflecting surface alpha
        // Angle of the draft surface psi
        const double alpha = -MFresnelLens::SlopeAngle(rc, F, N0, d);   // w.r.t. x  [30]
        const double psi   =  MFresnelLens::DraftAngle(r1);             // w.r.t. z  [ 5]

        const double tan_alpha = tan(alpha);
        const double tan_psi   = tan(psi);

        fGrooves[i].slope.z =  r0*tan_alpha;
        fGrooves[i].draft.z = -r1/tan_psi;

        fGrooves[i].slope.theta = TMath::Pi()/2-alpha;  // w.r.t. +z [ 60]
        fGrooves[i].draft.theta = -psi;                 // w.r.t. +z [- 5]

        fGrooves[i].slope.tan_theta = tan(fGrooves[i].slope.theta);
        fGrooves[i].draft.tan_theta = tan(fGrooves[i].draft.theta);

        fGrooves[i].slope.tan_theta2 = fGrooves[i].slope.tan_theta*fGrooves[i].slope.tan_theta;
        fGrooves[i].draft.tan_theta2 = fGrooves[i].draft.tan_theta*fGrooves[i].draft.tan_theta;

        fGrooves[i].slope.theta_norm = TMath::Pi()/2-fGrooves[i].slope.theta; // [ 30] 
        fGrooves[i].draft.theta_norm = TMath::Pi()/2-fGrooves[i].draft.theta; // [ 95] 

        const double dr = width/(tan_alpha*tan_psi+1);

        fGrooves[i].r = r0 + dr;

        const double z = -dr*tan_alpha;

        fGrooves[i].slope.h = z;
        fGrooves[i].draft.h = z;

        if (z<-fH)
            *fLog << warn << "Groove " << i << " deeper (" << z << ") than thickness of lens material (" << fH << ")." << endl;
    }

    fMaxR = (num+1)*width;
}

// --------------------------------------------------------------------------
//
// Reads the transmission curve from a file
// (tranmission in percent versus wavelength in nanometers)
//
// The transmission curve is used to calculate the absorption lengths.
// Therefore the thickness for which the tranission curve is valid is
// required (in cm).
//
// The conversion can correct for fresnel reflection at the entry and exit
// surface assuming that the outside material during the measurement was air
// (n=1.0003) and the material in PMMA. Correction is applied when
// correction is set to true <default>.
//
// If no valid data was read, 0 is returned. -1 is returned if any tranmission
// value read from the file is >1. If the fresnel correction leads to a value >1,
// the value is set to 1. The number of valid data points is returned.
//
Int_t MFresnelLens::ReadTransmission(const TString &file, float thickness, bool correction)
{
    TGraph transmission(file);

    /*
    double gx_min, gx_max, gy_min, gy_max;
    absorption.ComputeRange(gx_min, gy_min, gx_max, gy_max);
    if (lambda<gx_min || lambda>gx_max)
    {
        cout << "Invalid wavelength" << endl;
        return;
    }*/

    if (transmission.GetN()==0)
        return 0;

    for (int i=0; i<transmission.GetN(); i++)
    {
        // Correct transmission for Fresnel reflection on the surface
        const double lambda = transmission.GetX()[i];;

        double trans = transmission.GetY()[i];
        if (trans>1)
        {
            *fLog << err << "Transmission larger than 1." << endl;
            return -1;
        }

        if (correction)
        {
            // Something like this is requried if correction
            // for optical boundaries is necessary
            const double n0 = MFresnelLens::RefractiveIndex(lambda);

            // FIXME: Make N_air a variable
            const double r0 = (n0-1.0003)/(n0+1.0003);
            const double r2 = r0*r0;

            trans *= (1+r2)*(1+r2);

            if (trans>1)
            {
                *fLog << warn << "Transmission at " << lambda << "nm (" << trans << ") after Fresnel correction larger than 1." << endl;
                trans = 1;
            }
        }

        // convert to absorption length (FIMXE: Sanity check)
        transmission.GetY()[i] = -thickness/log(trans>0.999 ? 0.999 : trans);
    }

    fAbsorptionLength = MSpline3(transmission);

    return fAbsorptionLength.GetNp();
}

Int_t MFresnelLens::ReadEnv(const TEnv &env, TString prefix, Bool_t print)
{
    Bool_t rc = kFALSE;

    if (IsEnvDefined(env, prefix, "SurfaceRoughness", print))
    {
        rc = kTRUE;
        if (!GetEnvValue(env, prefix, "SurfaceRoughness", fPSF))
            return kERROR;
    }

    const int correction  = GetEnvValue(env, prefix, "Transmission.FresnelCorrection", -1);
    const float thickness = GetEnvValue(env, prefix, "Transmission.Thickness", -1.0); // [cm]
    const TString fname   = GetEnvValue(env, prefix, "Transmission.FileName", "");

    const bool correction_valid = correction>=0;
    const bool thickness_valid  = thickness>0;
    const bool fname_valid      = !fname.IsNull();

    if (!correction_valid && !thickness_valid && !fname_valid)
        return rc;

    if (correction_valid && thickness_valid && fname_valid)
        return ReadTransmission(fname, thickness, correction) >= 0 || rc;

    *fLog << err << "Reading transmission file required FileName, Thickness and FresnelCorrection." << endl;
    return kERROR;
}

// ==========================================================================

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

// --------------------------------------------------------------------------
//
// A Fresnel lens with parabolic surface calculated with the sagittag
// function (k=-1) and a correction for the thickness of the lens
// on the curvature. See also PhD thesis, Tim Niggemann ch. 7.1.1.
//
// see also W.J.Smith, Modern Optical Engineering, 2.8 The "Thin Lens"
// 1/f = (n-1)/radius   Eq. 2.36   with thickness t = 0
// bfl = f              Eq. 2.37    and R2 = inf (c2 = 0)
//
// Parameters are:
// The distance from the center r
// The focal length to be achieved F
// The refractive index of the outer medium (usually air) n0
// The refractive index of the lens material (e.g. PMMA) n1
// The thichness of the lens d
//
// r, F and d have to be in the same units.
//
// Return the slope angle alpha [rad]. The Slope angle is defined with
// respect to the plane of the lens. (0 at the center, decreasing
// with increasing radial distance)
//
double MFresnelLens::SlopeAngleParabolic(double r, double F, double n0, double n1, double d)
{
    // In the datasheet, it looks as if F is calculated
    // towards the center of the lens. It seems things are more
    // consistent if the thickness correction in caluating the
    // slope angle is omitted and the focal distance is measured
    // from the entrance of the lens => FIXME: To be checked
    const double rn = n1/n0;
    const double c = (rn - 1) * (F + d/rn); // FIXME: try and error with a large d
    return -atan(r/c);

    // F = 50.21
    //       d= 10        d=20
    // -:    47           43.7
    // 0:    53.5         57.0
    // +:    60.3         70.3
}

// --------------------------------------------------------------------------
//
// A Fresnel lens with an optimized parabolic surface calculated with
// the sagittag function (k=-1) and fitted coefficients according
// to Master thesis, Eichler.
//
// Note that for this setup other parameters must be fixed
//
// Parameters are:
// The distance from the center r
//
// r is in cm.
//
// Return the slope angle alpha [rad]. The Slope angle is defined with
// respect to the plane of the lens. (0 at the center, decreasing
// with increasing radial distance)
//
double MFresnelLens::SlopeAngleAspherical(double r)
{
    // Master, Eichler [r/cm]
    return -atan( r/26.47
                 +2*1.18e-4  * 1e1*r
                 +4*1.34e-9  * 1e3*r*r*r
                 +6*9.52e-15 * 1e5*r*r*r*r*r
                 -8*2.04e-19 * 1e7*r*r*r*r*r*r*r);
}

// --------------------------------------------------------------------------
//
// 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
//
// 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::SlopeAngleOptimized(double r, double F, double n)
{
    // Use F+d/2
    double so = r / sqrt(r*r + F*F);
    return atan(so / (1-sqrt(n*n - so*so))); // alpha<0, Range [0deg; -50deg]
}

// --------------------------------------------------------------------------
//
// Currently calles SlopeAngleParabolic(r, F, 1, n, d)
// Refractive Index air: https://refractiveindex.info/?shelf=other&book=air&page=Ciddor
//
double MFresnelLens::SlopeAngle(double r, double F, double n, double d)
{
    //return SlopeAngleAspherical(r);
    return SlopeAngleParabolic(r, F, 1.000278, n, d);
}


//
// 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]
}

// ==========================================================================

// --------------------------------------------------------------------------
//
// Return the total Area of all mirrors. Note, that it is recalculated
// with any call.
//
Double_t MFresnelLens::GetA() const
{
    return fMaxR*fMaxR*TMath::Pi();
}

// --------------------------------------------------------------------------
//
// Check with a rough estimate whether a photon can hit the reflector.
//
Bool_t MFresnelLens::CanHit(const MQuaternion &p) const
{
    // p is given in the reflectory coordinate frame. This is meant as a
    // fast check without lengthy calculations to omit all photons which
    // cannot hit the reflector at all
    return p.R2()<fMaxR*fMaxR;
}

// ==========================================================================

// FIXME: The rays could be 'reflected' inside the material
// (even though its going out) or vice versa
static double RandomTheta(double psf)
{
    return psf>0 ? MMath::RndmPSF(psf)/2 : 0;
}

// FIXME: The rays could be 'reflected' inside the material
// (even though its going out) or vice versa
static double RandomPhi(double r, double psf)
{
    return psf>0 ? MMath::RndmPSF(psf)/2 : 0;
}


// --------------------------------------------------------------------------
//
// Calculate the intersection point beweteen a line defined by the position p
// and the direction u and a cone defined by the object cone.
//
// Z: position of peak of cone
// theta: opening angle of cone
//
// Distance r of cone surface at given z from z-axis
// r_cone(z) = (Z-z)*tan(theta)
//
// Equalition of line
//  (x)   (p.x)          (u.x/u.z)
//  (y) = (p.y)  + dz *  (u.y/u.z)
//  (z)   (p.z)          (   1   )
//
// Normalization
//  U.x := u.x/u.z
//  U.y := u.y/u.z
//
// Distance of line at given z from z-axis
//  r_line(z) = sqrt(x^2 + y^2) = sqrt( (p.x+dz*u.x)^2 + (p.y+dz*u.y)^2)   with   dz = z-p.z
//
// Equation to be solved
//  r_cone(z) = r_line(z)
//
// Solved with wxmaxima:
//
//  [0] solve((px+(z-pz)*Ux)^2+(py+(z-pz)*Uy)^2= ((Z-z)*t)^2, z);
//
//  z= (sqrt(((Uy^2+Ux^2)*pz^2+(-2*Uy*py-2*Ux*px-2*Z*Uy^2-2*Z*Ux^2)*pz+py^2+2*Z*Uy*py+px^2+2*Z*Ux*px+Z^2*Uy^2+Z^2*Ux^2)*t^2-Ux^2*py^2+2*Ux*Uy*px*py-Uy^2*px^2)+Z*t^2+(-Uy^2-Ux^2)*pz+Uy*py+Ux*px)/(t^2-Uy^2-Ux^2),
//  z=-(sqrt(((Uy^2+Ux^2)*pz^2+(-2*Uy*py-2*Ux*px-2*Z*Uy^2-2*Z*Ux^2)*pz+py^2+2*Z*Uy*py+px^2+2*Z*Ux*px+Z^2*Uy^2+Z^2*Ux^2)*t^2-Ux^2*py^2+2*Ux*Uy*px*py-Uy^2*px^2)-Z*t^2+( Uy^2+Ux^2)*pz-Uy*py-Ux*px)/(t^2-Uy^2-Ux^2)
//
double MFresnelLens::CalcIntersection(const MQuaternion &p, const MQuaternion &u, const Cone &cone) const
{
    const double &Z = cone.z;

    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 &t  = cone.tan_theta;
    const double &t2 = cone.tan_theta2;

    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 - Z*t2;

    const double h  = Z-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;

    const double sqrt_radix = sqrt(radix);

    const double dz[2] =
    {
        (+sqrt_radix - b)/a,
        (-sqrt_radix - b)/a
    };

    // Return the closest solution inside the allowed range
    // which is in the direction of movement

    const double &H = cone.h;

    const bool is_inside0 = dz[0]>=H && dz[0]<0;
    const bool is_inside1 = dz[1]>=H && dz[1]<0;

    // FIXME: Simplify!
    if (!is_inside0 && !is_inside1)
        return 0;

    // Only dz[0] is in the right z-range
    if (is_inside0 && !is_inside1)
    {
        // Check if dz[0] is in the right direction
        if ((u.Z()>=0 && dz[0]>=p.Z()) ||
            (u.Z()< 0 && dz[0]< p.Z()))
            return dz[0];

        return 0;
    }

    // Only dz[1] is in the right z-range
    if (!is_inside0 && is_inside1)
    {
        // Check if dz[1] is in the right direction
        if ((u.Z()>=0 && dz[1]>=p.Z()) ||
            (u.Z()< 0 && dz[1]< p.Z()))
            return dz[1];

        return 0;
    }

    /*
    if (is_inside0^is_inside1)
    {
        if (u.Z()>=0)
            return dz[0]>p.Z() ? dz[0] : dz[1];
        else
            return dz[0]<p.Z() ? dz[0] : dz[1];
    }*/


    // dz[0] and dz[1] are in the right range
    // return the surface which is hit first

    // moving upwards
    if (u.Z()>=0)
    {
        // Both solution could be correct
        if (dz[0]>=p.Z() && dz[1]>=p.Z())
            return std::min(dz[0], dz[1]);

        // only one solution can be correct
        return dz[0]>=p.Z() ? dz[0] : dz[1];
    }
    else
    {
        // Both solution could be correct
        if (dz[0]<p.Z() && dz[1]<p.Z())
            return std::max(dz[0], dz[1]);

        // only one solution can be correct
        return dz[0]<p.Z() ? dz[0] : dz[1];
    }
}

// --------------------------------------------------------------------------
//
// Find the peak (draft+slope) which will be hit by the photon which
// is defined by position p and direction u. ix gives the index of the groove
// to originate the search from.
//
// Returns the index of the groove to which the surface belongs, -1 if no
// matching surface was found.
//
int MFresnelLens::FindPeak(size_t ix, const MQuaternion &p, const MQuaternion &u) const
{
    // ---------------------------
    // check for first groove first
    if (ix==0)
    {
        const auto test = p.fVectorPart + (fGrooves[0].slope.h-p.Z())/u.Z()*u.fVectorPart;
        if (test.XYvector().Mod()<fGrooves[0].r)
            return 0;
    }

    // r = sqrt( (px + t*ux) + (py + t*uy)^2 )
    // dr/dt = (2*uy*(dz*uy+py)+2*ux*(dz*ux+px))/(2*sqrt((dz*uy+py)^2+(dz*ux+px)^2))
    // dr/dt = (uy*py + ux*px)/sqrt(py^2+px^2)
    const bool outgoing = u.X()*p.X() + u.Y()*p.Y() > 0; // r is (at least locally) increasing

    // ---------------------------
    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 Ur2 = Ux*Ux + Uy*Uy;
    const double cr2 = Ux*py - Uy*px;
    const double pr2 = px*px + py*py;
    const double Up2 = Ux*px + Uy*py;

    //for (int i=1; i<fGrooves.size(); i++)

    // To speed up the search, search first along the radial moving direction of
    // the photon. If that was not successfull, try in the opposite direction.
    // FIXME: This could still fail in some very rare cases, for some extremely flat trajectories
    for (int j=0; j<2; j++)
    {
        const bool first = j==0;

        const int step = outgoing ^ !first ? 1 : -1;
        const int end  = outgoing ^ !first ? fGrooves.size() : 1;
        const int beg  = std::max<size_t>(j==0 ? ix : ix+step, 1);

        for (int i=beg; i!=end; i+=step)
        {
            const Groove &groove1 = fGrooves[i-1];
            const Groove &groove2 = fGrooves[i];

            const double &z1 = groove1.draft.h;
            const double &z2 = groove2.slope.h;

            const double &r1 = groove1.r;
            const double &r2 = groove2.r;

            Cone cone;
            cone.tan_theta  = -(r2-r1)/(z2-z1);
            cone.tan_theta2 = cone.tan_theta*cone.tan_theta;
            cone.z          = z1 + r1/cone.tan_theta;

            const double &Z  = cone.z;
            const double &t2 = cone.tan_theta2;

            const double a = t2 - Ur2;
            const double b = Ur2*pz - Up2 - Z*t2;

            const double h  = Z-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)
                continue;

            const double sqrt_radix = sqrt(radix);

            const double dz[2] =
            {
                (+sqrt_radix - b)/a,
                (-sqrt_radix - b)/a
            };

            if (dz[0]>=z2 && dz[0]<=z1)
                return i;

            if (dz[1]>=z2 && dz[1]<=z1)
                return i;
        }
    }

    return -1;
}

// --------------------------------------------------------------------------
//
// If no transmission was given returns true. Otherwaise calculates the
// absorption length for a flight time dt in the material and a photon
// with wavelength lambda. The flight time is converted to a geometrical
// using the speed of light in the medium.
//
// Returns true if the poton passed, false if it was absorbed.
//
bool MFresnelLens::Transmission(double dt, double lambda) const
{
    if (fAbsorptionLength.GetNp()==0)
        return true;

    // FIXME: Speed up!
    const double alpha = fAbsorptionLength.Eval(lambda);

    // We only have the travel time, thus we have to convert back to distance
    // Note that the transmission coefficients are w.r.t. to geometrical
    // distance not light-travel distance. Thus the distance has to be corrected
    // for the corresponding refractive index of the material.
    const double cm = dt/fVc;  

    const double trans = exp(-cm/alpha);
    return gRandom->Uniform()<trans;
}

/*
// surface=0 : incoming ray
// surface=1 : slope
// surface=2 : draft
// surface=3 : bottom
int MFresnelLens::EnterGroove(int surface, double n0, double lambda, MQuaternion &pos, MQuaternion &dir) const
{
    const double rx = pos.R();

    if (surface==3)
    {
        //cout << "Bottom as origin invalid" << endl;
        throw -1;

    }
    if (rx>=fR)
    {
        //cout << "Left the lens radius (enter)" << endl;
        throw -2;
    }
    //if (dir.Z()>0)
    //{
    //    cout << "Upgoing, outside of the material" << endl;
    //    PropagateZ(pos, dir, dir.Z()>0 ? 3 : -3);
    //    return -1;
    //}


   // Calculate the ordinal number of the groove correpsonding to rx
    const int ix = TMath::FloorNint(rx/fW);

    // Photons was just injected (test both surfaces) or came from the other surface
    if (surface==0 || surface==2)
    {
        // Get the possible intersection point with the slope angle
        const double z1 = CalcIntersection(pos, dir, fGrooves[ix].slope);

        // We hit the slope angle
        if (z1!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z1);

            if (fSlopeAbsorption)
                throw -100;

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(fGrooves[ix].slope.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, 1, n0);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return EnterGroove(1, n0, lambda, pos, dir)+1;

            // Transition was Refraction - enter
            if (ret>=3)
                return LeavePeak(1, n0, lambda, pos, dir, pos.T())+1;

            // Error occured (see ApplyTransition for details)
            //cout << "ERR[TIR1]" << endl;
            throw -3;
        }
    }

    // Photons was just injected (test both surfaces) or came from the other surface
    if (surface==0 || surface==1)
    {
        const double z2 = CalcIntersection(pos, dir, fGrooves[ix].draft);

        // We hit the draft angle
        if (z2!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z2);

            if (fDraftAbsorption)
                throw -101;

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(fGrooves[ix].draft.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, 1, n0);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return EnterGroove(2, n0, lambda, pos, dir)+1;

            // Transition was Refraction - enter
            if (ret>=3)
                return -LeavePeak(2, n0, lambda, pos, dir, pos.T())+1;

            // Error occured (see ApplyTransition for details)
            //cout << "ERR[TIR2]" << endl;
            throw -4;
        }
    }

    if (dir.Z()>0)
    {
        //cout << "Upgoing, outside of the material" << endl;
        //pos.PropagateZ(dir, dir.Z()>0 ? 3 : -3);
        throw -5;
    }

    // The ray has left the peak at the bottom(?)
    //cout << "ERR[N/A]" << endl;
    throw -6;
}
*/


// surface=0 : incoming ray
// surface=1 : slope
// surface=2 : draft
// surface=3 : bottom
int MFresnelLens::EnterGroove(int surface, double n0, MQuaternion &pos, MQuaternion &dir) const
{
    const double rx = pos.R();

    if (surface==kExitSurface)
        throw raytrace_error(kEnter+kInvalidOrigin, surface, -1,
                             "EnterGroove - Bottom as origin invalid");

    if (rx>=fR) // This is an error as the direction vector is now invalid
        throw raytrace_error(kEnter+kOutsideRadius, surface, -1,
                            "EnterGroove - Surface hit outside allowed radius");

    /*
    if (dir.Z()>0)
        return -1;
    }*/


    // FIXME: There is a very tiny chance that a ray hits the same surface twice for
    // very horizontal rays. Checking this needs to make sure that the same
    // solution is not just found again.

    // Calculate the ordinal number of the groove correpsonding to rx
    const int ix = TMath::FloorNint(rx/fW);

    // Photons was just injected (test both surfaces) or came from the other surface
    if (surface==kEntrySurface || surface==kDraftSurface)
    {
        // Get the possible intersection point with the slope angle
        const double z1 = CalcIntersection(pos, dir, fGrooves[ix].slope);

        // We hit the slope angle
        if (z1!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z1);
            if (fSlopeAbsorption)
                throw raytrace_user(kEnter+kAbsorbed, surface, kSlopeSurface,
                                    "EnterGroove - Photon absorbed by slope surface");

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(fGrooves[ix].slope.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, 1, n0, fFresnelReflection);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return kSlopeSurface;//EnterGroove(1, n0, lambda, pos, dir)+1;

            // Transition was Refraction - enter
            if (ret>=3)
                return -kSlopeSurface;//LeavePeak(1, n0, lambda, pos, dir, pos.T())+1;

            // Error occured (see ApplyTransition for details)
            throw raytrace_error(kEnter+kTransitionError, surface, kSlopeSurface,
                                 "EnterGroove - MOptics::ApplyTransition failed for slope surface");
        }
    }

    // Photons was just injected (test both surfaces) or came from the other surface
    if (surface==kEntrySurface || surface==kSlopeSurface)
    {
        const double z2 = CalcIntersection(pos, dir, fGrooves[ix].draft);

        // We hit the draft angle
        if (z2!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z2);
            if (fDraftAbsorption)
                throw raytrace_user(kEnter+kAbsorbed, surface, kDraftSurface,
                                    "EnterGroove - Photon absorbed by draft surface");

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(fGrooves[ix].draft.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, 1, n0, fFresnelReflection);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return kDraftSurface;//EnterGroove(2, n0, lambda, pos, dir)+1;

            // Transition was Refraction - enter
            if (ret>=3)
                return -kDraftSurface;//LeavePeak(2, n0, lambda, pos, dir, pos.T())+1;

            // Error occured (see ApplyTransition for details)
            throw raytrace_error(kEnter+kTransitionError, surface, kDraftSurface,
                                 "EnterGroove - MOptics::ApplyTransition failed for draft surface");
        }
    }

    if (dir.Z()>0)
    {
        // We have missed both surfaces and we are upgoing...
        // ... ray can be discarded
        throw raytrace_info(kEnter+kStrayUpgoing, surface, kNoSurface,
                            "EnterGroove - Particle is upgoing and has hit no surface");
    }

    // The ray has left the peak at the bottom(?)
    throw raytrace_error(kEnter+kStrayDowngoing, surface, kNoSurface,
                         "EnterGroove - Particle is downgoing and has hit no surface");
}

/*
// Leave the peak from inside the material, either thought the draft surface or the
// slope surface or the bottom connecting the valley of both
int MFresnelLens::LeavePeak(int surface, double n0, double lambda, MQuaternion &pos, MQuaternion &dir, double T0) const
{
    const double rx = pos.R();

    if (rx>=fR)
    {
        //cout << "Left the lens radius (leave)" << endl;
        throw -10;
    }

    if (dir.Z()>0 && surface!=3) // && surface!=4)
    {
        //cout << "Upgoing, inside of the material" << endl;
        //pos.PropagateZ(dir, dir.Z()>0 ? 3 : -3);
        throw -11;
    }

    if (surface!=1 && surface!=2 && surface!=3) // && surface!=4)
    {
        //cout << "Surface of origin invalid" << endl;
        throw -12;
    }


    // Calculate the ordinal number of the groove correpsonding to rx
    const int ix = TMath::FloorNint(rx/fW);

    // FIXME: The Z-coordinate (cone.h) is actually a line through two points!!!

    Cone slope = fGrooves[ix].slope;
    Cone draft = fGrooves[ix].draft;

    const bool is_draft = rx>fGrooves[ix].r;
    if (is_draft)
    {
        // We are in the volume under the draft angle... taking the slope from ix+1
        if (ix<fGrooves.size()-1) // FIXME: Does that make sense?
            slope = fGrooves[ix+1].slope;
    }
    else
    {
        // We are in the volume under the slope angle... taking the draft from ix-1
        if (ix>0) // FIXME: Check whether this is correct
            draft = fGrooves[ix-1].draft;
    }

    if (is_draft+1!=surface && (surface==1 || surface==2))
        cout << "SURFACE: " << is_draft+1 << " " << surface << endl;

    if (surface==3)
    {
        //cout << "Upgoing, coming from the bottom of the lens" << endl;
        // Find out which triangle (peak) the photon is going to enter
        // then proceed...
        throw -13;
    }


    // We are inside the material and downgoing, so if we come from a slope surface,
    // we can only hit a draft surface after and vice versa
    if (is_draft || surface==3)
    {
        const double z1 = CalcIntersection(pos, dir, slope);

        // We hit the slope angle and are currently in the volume under the draft surface
        if (z1!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z1);

            if (fSlopeAbsorption)
                throw -200;

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(slope.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, n0, 1);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return LeavePeak(1, n0, lambda, pos, dir, T0)+1;

            // Transition was Refraction - leave
            if (ret>=3)
            {
                if (!Transmission(pos.T()-T0, lambda))
                    throw -14;

                return EnterGroove(1, n0, lambda, pos, dir)+1;
            }

            // Error occured (see ApplyTransition for details)
            //cout << "ERR[TIR3]" << endl;
            throw -15;
        }
    }

    if (!is_draft || surface==3)
    {
        const double z2 = CalcIntersection(pos, dir, draft);

        // We hit the draft angle from the inside and are currently in the volume under the slope angle
        if (z2!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z2);

            if (fDraftAbsorption)
                throw -201;

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(draft.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, n0, 1);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return LeavePeak(2, n0, lambda, pos, dir, T0)+1;

            // Transition was Refraction - leave
            if (ret>=3)
            {
                if (!Transmission(pos.T()-T0, lambda))
                    throw -16;

                return EnterGroove(2, n0, lambda, pos, dir)+1;
            }

            // Error occured (see ApplyTransition for details)
            //cout << "ERR[TIR4]" << endl;
            throw -17;
        }
    }

    if (surface==3)// || surface==4)
    {
        //cout << ix << " Lost bottom reflected ray " << surface << endl;
        throw -18;
    }

    // The ray has left the peak at the bottom

    // FIXME: There is a tiny chance to escape to the side
    // As there is a slope in the bottom surface of the peak

    // Move photon to new hit position
    pos.PropagateZ(dir, -fH);

    if (pos.R()>fR)
    {
        //cout << "Left the lens radius (bottom)" << endl;
        throw -19;
    }

    // Get the normal vector of the surface which was hit
    const VectorNorm norm(RandomTheta(fPSF), gRandom->Uniform(0, TMath::TwoPi()));

    // Get the optical transition of the direction vector
    const int ret = MOptics::ApplyTransition(dir, norm, n0, 1);

    // Transition was Reflection
    // (Photon scattered back from the bottom of the lens)
    if (ret==1 || ret==2)
        return LeavePeak(3, n0, lambda, pos, dir, T0)+1;

    // Transition was Refraction
    // (Photon left at the bottom of the lens)
    if (ret>=3)
    {
        if (!Transmission(pos.T()-T0, lambda))
            throw -20;

        return 0;
    }

    // Error occured (see ApplyTransition for details)
    //cout << "ERR[TIR5]" << endl;
    throw -21;
}*/

// Leave the peak from inside the material, either thought the draft surface or the
// slope surface or the bottom connecting the valley of both
int MFresnelLens::LeavePeak(int surface, double n0, MQuaternion &pos, MQuaternion &dir, double T0) const
{
    const double rx = pos.R();

    if (rx>=fR) // This is an error as the direction vector is now invalid
        throw raytrace_error(kLeave+kOutsideRadius, surface, kNoSurface,
                             "LeavePeak - Surface hit outside allowed radius");

    // FIXME: Can we track them further?
    if (fDisableMultiEntry && dir.Z()>0 && surface!=3/* && surface!=4*/)
        throw raytrace_info(kLeave+kStrayUpgoing, surface, kNoSurface,
                            "LeavePeak - Particle is upgoing inside the material and does not come from the bottom");

    if (surface!=kSlopeSurface && surface!=kDraftSurface && surface!=kExitSurface/* && surface!=4*/)
        throw raytrace_error(kLeave+kInvalidOrigin, surface, kNoSurface,
                             "LeavePeak - Invalid surface of origin");


    // Calculate the ordinal number of the groove correpsonding to rx
    const uint32_t ix = TMath::FloorNint(rx/fW);

    // FIXME: The Z-coordinate (cone.h) is actually a line through two points!!!

    Cone slope = fGrooves[ix].slope;
    Cone draft = fGrooves[ix].draft;

    //if (is_draft+1!=surface && (surface==1 || surface==2))
    //    cout << "SURFACE: " << is_draft+1 << " " << surface << endl;

    const bool is_draft = rx>fGrooves[ix].r;
    if (is_draft)
    {
        // We are in the volume under the draft angle... taking the slope from ix+1
        if (ix<fGrooves.size()-1) // FIXME: Does that make sense?
            slope = fGrooves[ix+1].slope;
    }
    else
    {
        // We are in the volume under the slope angle... taking the draft from ix-1
        if (ix>0) // FIXME: Check whether this is correct
            draft = fGrooves[ix-1].draft;
    }

    if (surface==kExitSurface)
    {
        if (!fBottomReflection)
            throw raytrace_user(kLeave+kAbsorbed, surface, kExitSurface,
                                "LeavePeak - Particle absorbed on the bottom");

        const int in = FindPeak(ix, pos, dir);

        // This might happen if the ray is very flat and leaving
        // the lens before hitting the border boundary of the grooves
        if (in<0)
            throw raytrace_error(kLeave+kNoSurfaceFound, kExitSurface, kNoSurface,
                                 "LeavePeak - No hit surface found for particle reflected at the bottom");

        slope = fGrooves[in].slope;
        draft = fGrooves[in==0 ? 0 : in-1].draft;
    }

    // FIXME: There is a chance that we can hit the same surface twice (for very horizontal rays
    //        but this requires a proper selection of the hit point

    // We are inside the material and downgoing, so if we come from a slope surface,
    // we can only hit a draft surface after and vice versa
    if (is_draft || surface==kExitSurface)
    {
        const double z1 = CalcIntersection(pos, dir, slope);

        // We hit the slope angle and are currently in the volume under the draft surface
        if (z1!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z1);

            if (fSlopeAbsorption)
                throw raytrace_user(kLeave+kAbsorbed, surface, kSlopeSurface,
                                    "LeavePeak - Photon absorbed by slope surface");

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(slope.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, n0, 1, fFresnelReflection);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return -kSlopeSurface;//LeavePeak(1, n0, lambda, pos, dir, T0)+1;

            // Transition was Refraction - leave
            if (ret>=3) // Transmission
                return kSlopeSurface;//EnterGroove(1, n0, lambda, pos, dir)+1;

            // Error occured (see ApplyTransition for details)
            throw raytrace_error(kLeave+kTransitionError, surface, kSlopeSurface,
                                 "LeavePeak - MOptics::ApplyTransition failed for slope surface");
        }
    }

    if (!is_draft || surface==kExitSurface)
    {
        const double z2 = CalcIntersection(pos, dir, draft);

        // We hit the draft angle from the inside and are currently in the volume under the slope angle
        if (z2!=0)
        {
            // Move photon to new hit position
            pos.PropagateZ(dir, z2);

            if (fDraftAbsorption)
                throw raytrace_user(kLeave+kAbsorbed, surface, kDraftSurface,
                                    "LeavePeak - Photon absorbed by draft surface");

            // Get the normal vector of the surface which was hit
            const VectorNorm norm(draft.theta_norm+RandomTheta(fPSF),
                                  pos.XYvector().Phi()+RandomPhi(pos.R(), fPSF));

            // Get the optical transition of the direction vector
            const int ret = MOptics::ApplyTransition(dir, norm, n0, 1, fFresnelReflection);

            // Transition was Reflection - try again
            if (ret==1 || ret==2)
                return -kDraftSurface;//LeavePeak(2, n0, lambda, pos, dir, T0)+1;

            // Transition was Refraction - leave
            if (ret>=3) // Transmission
                return kDraftSurface;//EnterGroove(2, n0, lambda, pos, dir)+1;

            // Error occured (see ApplyTransition for details)
            //cout << "ERR[TIR4]" << endl;
            throw raytrace_error(kLeave+kTransitionError, surface, kDraftSurface,
                                 "LeavePeak - MOptics::ApplyTransition failed for draft surface");
        }
    }

    if (surface==kExitSurface/* || surface==4*/)
        throw raytrace_error(kLeave+kFoundSurfaceUnavailable, kExitSurface, is_draft?kSlopeSurface:kDraftSurface,
                             "LeavePeak - Ray reflected on the bottom did not hit the found surface");

    // The ray has left the peak at the bottom

    // FIXME: There is a tiny chance to escape to the side
    // As there is a slope in the bottom surface of the peak

    // FIXME: Theoretically, a ray can hit the same surface twice

    // Move photon to new hit position
    pos.PropagateZ(dir, -fH);

    if (pos.R()>fR)
        throw raytrace_info(kLeave+kOutsideRadius, surface, kExitSurface,
                            "LeavePeak - Hit point at the bottom surface is beyond allowed radius");

    // Get the normal vector of the surface which was hit
    const VectorNorm norm(RandomTheta(fPSF), gRandom->Uniform(0, TMath::TwoPi()));

    // Get the optical transition of the direction vector
    const int ret = MOptics::ApplyTransition(dir, norm, n0, 1, fFresnelReflection);

    // Transition was Reflection
    // (Photon scattered back from the bottom of the lens)
    if (ret==1 || ret==2)
        return -kExitSurface;//LeavePeak(3, n0, lambda, pos, dir, T0)+1;

    // Transition was Refraction
    // (Photon left at the bottom of the lens)
    if (ret>=3) // Transmission
        return kPhotonHasLeft;

    // Error occured (see ApplyTransition for details)
    throw raytrace_error(kLeave+kTransitionError, surface, kExitSurface, "LeavePeak - MOptics::ApplyTransition failed for bottom surface");
}


// Differences:
// Returns a 'reflected' vector at z=0
// Does not propagate to z=0 at the beginning
Int_t MFresnelLens::ExecuteOptics(MQuaternion &p, MQuaternion &u, const Short_t &wavelength) const
{
    // Corsika Coordinates are in cm!

    const double lambda = wavelength==0 ? fLambda : wavelength;
    if (fAbsorptionLength.GetNp()!=0 &&
        (lambda<fAbsorptionLength.GetXmin() || lambda>fAbsorptionLength.GetXmax()))
        {
            *fLog << err << "Wavelength " << lambda << "nm out of absorption range [" << fAbsorptionLength.GetXmin() << "nm;" << fAbsorptionLength.GetXmax() << "nm]" << endl;
            return -1;
        }

    const double n0 = MFresnelLens::RefractiveIndex(lambda);

    try
    {
        int last_surface = kEntrySurface;//EnterGroove(kEntrySurface, n0, p, u);

        // last_surface that was hit (photon originates from)
        //  0 entrance (Z=0) or exit (Z=-fH) surface
        //  1 slope
        //  2 draft
        //  3 bottom
        // positive: photon is outside of material  -->  Try to enter
        // nagative: photon is inside  of material  -->  Try to leave

        double T0 = 0;//last_surface<0 ? p.T() : 0;

        // The general assumption is: no surface can be hit twice in a row

        int cnt = -1;
        while (last_surface!=0)
        {
            cnt ++;

            // photon is outside of material --> try to enter
            if (last_surface>0)
            {
                last_surface = EnterGroove(last_surface, n0, p, u);

                // successfully entered --> remember time of entrance to calculate transimission
                if (last_surface<0)
                    T0 = p.T();

                continue;
            }

            // photon is inside of material --> try to leave
            if (last_surface<0)
            {
                last_surface = LeavePeak(-last_surface, n0, p, u, T0);

                // successfully left --> apply transmission
                if (last_surface>=0)
                {
                    if (!Transmission(p.T()-T0, lambda))
                        throw raytrace_error(kAbsorbed, last_surface, kMaterial,
                                             "TraceRay - Ray absorbed in material");
                }

                continue;
            }
        }

        // To make this consistent with a mirror system,
        // we now change our coordinate system
        // Rays from the lens to the camera are up-going (positive sign)
        u.fVectorPart.SetZ(-u.Z());

        // In the datasheet, it looks as if F is calculated
        // towards the center of the lens. It seems things are more
        // consistent if the thickness correction in caluating the
        // slope angle is omitted and the focal distance is measured
        // from the entrance of the lens => FIXME: To be checked
        // (Propagating to F means not propagating a distance of F-H from the exit)
        //p.fVectorPart.SetZ(fH-fH/2/fN);//fH/2); Found by try-and-error

        // We are already at -H, adding F and setting Z=0 means going to -(F+H)
        p.fVectorPart.SetZ(0);//fH/2); Found by try-and-error

        return uint32_t(cnt)>=fMinHits && (fMaxHits==0 || uint32_t(cnt)<=fMaxHits) ? cnt : -1;;
    }
    catch (const raytrace_exception &e)
    {
        return -e.id();
    }

/*
    try
    {
        const int cnt = EnterGroove(0, n0, lambda, p, u);

        // To make this consistent with a mirror system,
        // we now change our coordinate system
        // Rays from the lens to the camera are up-going (positive sign)
        u.fVectorPart.SetZ(-u.Z());

        // In the datasheet, it looks as if F is calculated
        // towards the center of the lens
        // (Propagating to F means not propagating a distance of F-H/2)
        p.fVectorPart.SetZ(0);

        return cnt>=fMinHits && (fMaxHits==0 || cnt<=fMaxHits) ? cnt : -1;

    }
    catch (const int &rc)
    {
        return rc;
    }
*/
}

// Differences:
// Does propagate to z=0 at the beginning
Int_t MFresnelLens::TraceRay(vector<MQuaternion> &vec, MQuaternion &p, MQuaternion &u, const Short_t &wavelength, bool verbose) const
{
    // Corsika Coordinates are in cm!

    const double lambda = wavelength==0 ? fLambda : wavelength;
    if (fAbsorptionLength.GetNp()!=0 &&
        (lambda<fAbsorptionLength.GetXmin() || lambda>fAbsorptionLength.GetXmax()))
        {
            *fLog << err << "Wavelength " << lambda << "nm out of absorption range [" << fAbsorptionLength.GetXmin() << "nm;" << fAbsorptionLength.GetXmax() << "nm]" << endl;
            return -1;
        }

    const double n0 = MFresnelLens::RefractiveIndex(lambda);

    // Photon must be at the lens surface
    p.PropagateZ(u, 0);
    vec.push_back(p);

    try
    {
        int last_surface = kEntrySurface;//EnterGroove(kEntrySurface, n0, p, u);

        // last_surface that was hit (photon originates from)
        //  0 entrance (Z=0) or exit (Z=-fH) surface
        //  1 slope
        //  2 draft
        //  3 bottom
        // positive: photon is outside of material  -->  Try to enter
        // nagative: photon is inside  of material  -->  Try to leave

        double T0 = 0;

        // The general assumption is: no surface can be hit twice in a row

        int cnt = -1;
        while (last_surface!=0)
        {
            cnt ++;
            vec.push_back(p);

            // photon is outside of material --> try to enter
            if (last_surface>0)
            {
                last_surface = EnterGroove( last_surface, n0, p, u);
                //cout << "enter = " << last_surface << endl;

                // successfully entered --> remember time of entrance to calculate transimission
                if (last_surface<0)
                    T0 = p.T();

                continue;
            }

            // photon is inside of material --> try to leave
            if (last_surface<0)
            {
                last_surface = LeavePeak(-last_surface, n0, p, u, T0);
                //cout << "leave = " << last_surface << endl;

                // successfully left --> apply transmission
                if (last_surface>=0)
                {
                    if (!Transmission(p.T()-T0, lambda))
                        throw raytrace_error(kAbsorbed, last_surface, kMaterial,
                                             "TraceRay - Ray absorbed in material");
                }

                continue;
            }
        }

        vec.push_back(p);
        return cnt;
    }
    catch (const raytrace_exception &e)
    {
        if (verbose)
            *fLog << all << e.id() << ": " << e.what() << endl;

        // Hit point at bottom surface beyond allowed range
        // FIXME: Only if surface is kExitSurface
        if (e.id()==2342)
            vec.push_back(p);

        return -e.id();
    }
}