/* ======================================================================== *\
!
! *
! * 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-2009
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
//  MFresnelLens
//
//////////////////////////////////////////////////////////////////////////////
#include "MFresnelLens.h"

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

#include "MQuaternion.h"

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

ClassImp(MFresnelLens);

using namespace std;

// --------------------------------------------------------------------------
//
// Default constructor
//
MFresnelLens::MFresnelLens(const char *name, const char *title)
{
    fName  = name  ? name  : "MFresnelLens";
    fTitle = title ? title : "Parameter container storing a collection of several mirrors (reflector)";

    fMaxR = 55/2.;
}

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

struct Groove
{
    double fTheta;
    double fPeakZ;

    double fTanTheta;
    double fTanTheta2[3];

    Groove() { }
    Groove(double theta, double z) : fTheta(theta), fPeakZ(z)
    {
        fTanTheta = tan(theta);

        fTanTheta2[0] = fTanTheta*fTanTheta;
        fTanTheta2[1] = fTanTheta*fTanTheta * fPeakZ;
        fTanTheta2[2] = fTanTheta*fTanTheta * fPeakZ*fPeakZ;
    }
};

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)

   // Assuming a cone with peak at z and opening angle theta

   // Radius at distance z+dz from the tip and at distance r from the axis of the cone
   // r = (z+dz) * tan(theta)

   // Defining
   // zc := z+dz

   // Surface of the cone
   // (X)         ( tan(theta) * sin(phi) )
   // (Y)  = zc * ( tan(theta) * cos(phi) )
   // (Z)         (          1            )

   // Line
   // (X)        (u.x)   (p.x)
   // (Y) = dz * (u.y) + (p.y)
   // (Z)        (u.z)   ( 0 )

   // Equality
   //
   // X^2+Y^2 = r^2
   //
   // (dz*u.x+p.x)^2 + (dz*u.y+p.y)^2 = (z+dz)^2 * tan(theta)^2
   //
   // dz^2*ux^2 + px^2 + 2*dz*ux*px + dz^2*uy^2 + py^2 + 2*dz*uy*py = (z^2*tan(theta)^2 + dz^2*tan(theta)^2 + 2*z*dz*tan(theta)^2
   //
   // dz^2*(ux^2 + uy^2 - tan(theta)^2)  +  2*dz*(ux*px + uy*py - z*tan(theta)^2)  +  (px^2+py^2 - z^2*tan(theta)^2)  =  0

   // t   := tan(theta)
   // a   := ux^2    + uy^2    -     t^2
   // b/2 := ux*px   + uy*py   - z  *t^2 := B
   // c   :=    px^2 +    py^2 - z^2*t^2

   // dz = [ -b +- sqrt(b^2 - 4*a*c) ] / [2*a]
   // dz = [ -B +- sqrt(B^2 -   a*c) ] /    a

    const double a = u.R2()                    - g.fTanTheta2[0];
    const double B = u.XYvector()*p.XYvector() - g.fTanTheta2[1];
    const double c = p.R2()                    - g.fTanTheta2[2];

    const double B2 = B*B;
    const double ac = a*c;

    if (B2<ac)
        return 0;

    const double sqrtBac = sqrt(B2 - a*c);

    if (a==0)
        return 0;

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

    // Only one dz[i] can be within the lens (due to geometrical reasons

    if (dz[0]<0 && dz[0]>fThickness)
        return dz[0];

    if (dz[1]<0 && dz[1]>fThickness)
        return dz[1];

    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
{
    // Corsika Coordinates are in cm!

    const double D = 54.92;  // [cm] Diameter
    const double F = 50.21;  // [cm] focal length (see also MGeomCamFAMOUS!)

    const double R = D/2;    // [cm] radius of lens
    const double w = 0.0100; // [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);

    // Ray has missed the lens
    if (p.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]

    // Find dw value of common z-value
    //
    // tan(90-psi)  =  z/dw
    // tan(alpha)   = -z/(w-dw)
    //
    // -> 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)
    // FIXME: Surace roughness
    if (!ApplyRefraction(u, norm, 1, n))
        return -1;

    // 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)
    // FIXME: Surace roughness
    if (!ApplyRefraction(u, TVector3(0, 0, 1), n, 1))
        return -1;

    // 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.
    p.fVectorPart.SetZ(-H/2);

    return nth; // Keep track of groove index
}