#include <G4GeometryTolerance.hh>
#include <RAT/QuadraticRoots.hh>
#include <RAT/SNOSVBellyPlate.hh>
#include <RAT/SphericalSubregionList.hh>
#include <RAT/Log.hh>
#include <CLHEP/Units/PhysicalConstants.h>
using namespace RAT;

SNOSVBellyPlate::SNOSVBellyPlate(const G4double rmin, const G4double rmax, const G4ThreeVector &xAxis, const G4ThreeVector &yAxis, const G4ThreeVector &zAxis, const G4double zInner,
            const G4double zOuter):
            fXaxis(xAxis), fYaxis(yAxis),fZaxis(zAxis){  //y axis goes through the center of the belly plate; x axis perpendicular and z vertical
    fThreshold = G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
    fRmin = rmin; fRmax = rmax; fZInner = zInner; fZOuter = zOuter;
    fRhomin = sqrt(fRmin*fRmin - fZInner*fZInner);
    fRhomax = sqrt(fRmax*fRmax - fZOuter*fZOuter);
    fRhoSlope = (fRhomax - fRhomin) / (fZOuter - fZInner);
    fZOffset = fZInner - fRhomin / fRhoSlope;
    fYmin = fRmax*cos(CLHEP::pi/10.); // Using half the angular distance between belly plates
}

G4double SNOSVBellyPlate::Rho(G4double z)const {  //returns cylindrical radius for a given z
    G4double rho;
    if (z<0)z = -z;
    if (z<fZInner)rho = sqrt(fRmin*fRmin - z*z);
    else
      if (z>fZOuter)rho = sqrt(fRmax*fRmax - z*z);
      else rho = (z - fZOffset)*fRhoSlope;
    return rho;
}

G4double SNOSVBellyPlate::Radius(G4double z)const {  //returns spherical radius for a given z
    G4double radius;
    double rho;
    if (z<0)z = -z;
    if (z<fZInner)radius = fRmin;
    else
      if (z>fZOuter)radius = fRmax;
      else { rho = (z - fZOffset)*fRhoSlope;  radius = sqrt(rho*rho + z*z); }
    return radius;
}

EInside SNOSVBellyPlate::Inside(const G4ThreeVector &p)const {
    EInside in;
    double x = p*fXaxis;  double z = p*fZaxis; // double y = p*fYaxis;
    double r = Radius(x); double r2 = Radius(z);
    if (r2>r)r = r2;  // always pick the maximum radius to deal with the curvature in both x and z directions.
    double delta = p.mag() - r;
    // Photons can end up inside a "mirror" belly plate because this only checks quantities without a sign
    // DistanceToIn function checks that the proposed distances are inside, but mirror belly plates aren't discarded
    // Adding a first test to know if the photon is inside this belly plate region at all

    if (delta< -fThreshold)in = kInside;
    else
      if (delta>fThreshold)in = kOutside;
      else in = kSurface;
    return in;
}


G4ThreeVector SNOSVBellyPlate::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
    G4ThreeVector normal=-p.unit();
    isValid = true; // we set true , to allow us to return at any point. However, if we find that the point is not "within" the belly plate, we need to set it false.
    double x = p*fXaxis;  double z = p*fZaxis; double y = p*fYaxis;
    double r1 = Radius(x); double r2 = Radius(z);
    double r = r2>r1 ? r2 : r1; // always pick the maximum radius to deal with the curvature in both x and z directions.
    if (fabs(r - fRmin)<fThreshold || fabs(r - fRmax)<fThreshold) return p.unit();
    if (r1>r2){ //on one of the x-cones.  For a cone oriented along the x-axis, with the point at the origin and we have rho=alpha*x=sqrt(y*y+z*z);
        // The normal to a point on the cone (x,y,z)=(x,rho*cos(phi),rho*sin(phi))=(x,alpha*x*cos(phi),alpha*x*sin(phi))
        // is (x,-x/alpha*cos(phi),-x/alpha*sin(phi));
        // JP mod - this should be atan2(z,y) already transformed (See SNOAVBellyPlate:165)
        double phi = atan2(z,y);
        if (x>0){
            // JP mod - this recipe was fine, but it was not rotated afterwards
            // Implementing the SNOAVBellyPlate formulae, which is rotated appropriately
            normal = (-fXaxis+1/ fRhoSlope*cos(phi)*fYaxis+1 / fRhoSlope*sin(phi)*fZaxis).unit();
        }
        else{
            normal = (fXaxis+1/ fRhoSlope*cos(phi)*fYaxis+1 / fRhoSlope*sin(phi)*fZaxis).unit();
        }
    }
    else{  //on one of the z-cones
        double phi = p.phi();
        // JP - for cones in z the system doesn't need rotations, so it was fine before.
        // Changing it for consistency
        if (z>0){
            normal = G4ThreeVector(1.0 / fRhoSlope*cos(phi), 1.0 / fRhoSlope*sin(phi), -1).unit();
        }
        else{
            normal = G4ThreeVector(1.0 / fRhoSlope*cos(phi), 1.0 / fRhoSlope*sin(phi), 1).unit();
        }
    }
    return normal.unit();
}


G4double SNOSVBellyPlate::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double *, const G4double dOuter[2])const {
    // find the intersections with the surface of the belly plate.
    //Check the intersections with the inner radius
    // Note this routine ignores the proposed distance-- it finds the qualified intersection with all the surfaces of the bellyplate, and resets qualifiedDistance if smaller than before.
    double dist,dtest;
    G4ThreeVector test;
    int i;
    double x, z;
    // note that dInner for the SNOSV class refers to the id of the NCD's.  Loosing irrelevant generality,
    // we make use of that fact by recalculating dinner for the Bellyplate.
    double dInner[2];
    SphericalSubregionList::CalculateSphereIntersection(p,n,fRmin,dInner);

    // JP - if the test only checks the shortest dInner then you dont consider cases when you start near a belly plate
    // and cross to the other side, where the belly plate being tested is. Need to loop over dInner for both possibilities.
    //if(dInner[0]>fThreshold)dtest=dInner[0];
    //else dtest=dInner[1];
    dist=kInfinity;

    int nref;
    for(nref=0; nref<2; nref++){
      dtest = dInner[nref];
      if (dtest>fThreshold && dtest<dist){
        test = p + dtest * n;
        if(test*fYaxis>fYmin){
          x = fabs(test*fXaxis); z = fabs(test*fZaxis);
          double rr = test.mag();
          if (fabs(rr - fRmin)<fThreshold && x<fZInner &&z <fZInner)dist=dtest;
        }
      }
    }

    for(nref=0; nref<2;nref++){
      dtest = dOuter[nref];
      if (dtest>fThreshold&&dtest<dist){
        test = p + dtest * n;
        if(test*fYaxis>fYmin){
          x = fabs(test*fXaxis); z = fabs(test*fZaxis);
          double rr = test.mag();
          // JP mod - either x>fZOuter or z>fZouter means being outside the belly plate region
          if (fabs(rr - fRmax)<fThreshold && (x>fZOuter || z >fZOuter))dist=dtest;
        }
      }
    }

    //calculate conical intersections : first Z-cones since this doesn't require any rotation
    //double a, b, c, q, d;
    double a, b, c;             // WJH mod
    double a2 = fRhoSlope*fRhoSlope;
    z = p.z() - fZOffset;
    for (i = -1; i<2; i += 2){
        z = p.z() + i*fZOffset;
        b = 2 * (p.x()*n.x() + p.y()*n.y() - a2*n.z()*z);
        c = p.x()*p.x() + p.y()*p.y() - a2*z*z;
        a = 1 - (1 + a2)*n.z()*n.z();

        // WJH mod
        double d1, d2;
        if( QuadraticRoots( a, b, c, d1, d2) ){
          if(d1<d2) { double temp=d1; d1=d2; d2=temp;}
          if (d1>fThreshold && d1<dist){
            test = p + d1*n;
            if (Inside(test) == kSurface && test*fYaxis > fYmin) dist = d1;
          }
          if (d2>fThreshold && d2<dist){
            test = p + d2*n;
            if (Inside(test) == kSurface && test*fYaxis > fYmin) dist = d2;
          }
        }

    }
    // Now do X-axis cones
    G4ThreeVector px(-p*fZaxis, p*fYaxis, p*fXaxis);
    G4ThreeVector nx(-n*fZaxis, n*fYaxis, n*fXaxis);
    for (i = -1; i<2; i += 2){
        z = px.z() + i*fZOffset;
        b = 2 * (px.x()*nx.x() + px.y()*nx.y() - a2*nx.z()*z);
        c = px.x()*px.x() + px.y()*px.y() - a2*z*z;
        a = 1 - (1 + a2)*nx.z()*nx.z();

        // WJH mod
        double d1, d2;
        if( QuadraticRoots( a, b, c, d1, d2) ){
          if(d1<d2) { double temp=d1; d1=d2; d2=temp;}
          if (d1>fThreshold && d1<dist){
            test = p + d1*n;
            if (Inside(test) == kSurface && test*fYaxis > fYmin) dist = d1;
          }
          if (d2>fThreshold && d2<dist){
            test = p + d2*n;
            if (Inside(test) == kSurface && test*fYaxis > fYmin) dist = d2;
          }
        }
    }
    return dist;
}



G4double SNOSVBellyPlate::DistanceToOut(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{
    // since we are on a surface, there is no need to distinguish between in and out.
    return DistanceToIn(p, n, dInner, dOuter);
}

G4bool SNOSVBellyPlate::OverlapsRegion(const G4ThreeVector corners[4])const {
    double centertheta = fYaxis.theta(); double centerphi = fYaxis.phi();  //belly plate y axis goes through middle of panel
    double ctmin, ctmax, phmin, phmax;
    ctmin = cos(centertheta + asin(fZOuter / fRmax));
    ctmax = cos(centertheta - asin(fZOuter / fRmax));
    phmin = centerphi - asin(fZOuter / fRmax);
    phmax = centerphi + asin(fZOuter / fRmax);
    if (phmax > CLHEP::pi)phmax -= CLHEP::twopi;
    int i;
    bool retval=false;
    for(i=0;!retval && i<4;i++){
        retval=(corners[i].cosTheta()>ctmin&&corners[i].cosTheta()<ctmax &&corners[i].phi()>phmin &&corners[i].phi()<phmax);
    }
    return retval;
}