#include <RAT/QuadraticRoots.hh>
#include <RAT/SNONCDAnchor.hh>
#include <RAT/SphericalSubregionList.hh>
#include <G4GeometryTolerance.hh>

using namespace RAT;


SNONCDAnchor::SNONCDAnchor(G4double rmin, G4double rmax, G4double radius, const G4ThreeVector &position):fPosition(position){
    fDirection = position.unit();
    fRmin = rmin; fRmax = rmax; fRadius = radius;
    fThreshold = G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
}

EInside SNONCDAnchor::Inside(const G4ThreeVector &p)const {
  // JP - Adding a surface possibility (NCD top) and handling the non-NCD surface
    EInside in;
    double radius = Radius(p); double z=p.mag();
    z=p.mag();

    //JP addition
    if(z<fRmin-fThreshold) return kInside;
    else
      if(z>fRmax+fThreshold) return kOutside;

    if (radius<fRadius - fThreshold  &&p*fDirection>0){
        if(z>fRmin + fThreshold) in=kOutside;
        else in=kSurface;
    }
    else
      if (radius>fRadius + fThreshold){
        // JP mods - if not really on the NCD, still check for the SV std volume
        if(z<fRmax-fThreshold) in=kInside;
        else in=kSurface;
      }
    // Somewhere in the cylinder surface
      else in = kSurface;
    return in;
}

G4ThreeVector SNONCDAnchor::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
    // JP - Multiplied skew times -1. Surface normal was pointing in the wrong direction
    G4ThreeVector skew = (p - fPosition); skew = -1.*(skew - (skew*fDirection)*fDirection);  //vector perpendicular to cylinder axis, through p.
    double radius = skew.mag();  double z = p.mag();  if(p*fDirection<0)z=-z;
    const G4ThreeVector *retval;
    if (radius < fRadius + fThreshold  && z>fRmin - fThreshold && z < fRmax + fThreshold){
        isValid = true;
        if (z<fRmin + fThreshold)retval = &fDirection;
        else retval = &skew;
    }else{
        isValid = false; //p is not on the actual surface of this block
                retval=&skew;
    }
    return retval->unit();
}

G4double SNONCDAnchor::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{
    double dist=kInfinity;
    G4ThreeVector test = n.cross(fDirection); test = test.unit();
    if (fabs(test*(p - fPosition))>fRadius)return dist;  // no intersection possible

    double dtest, z;

    // JPmod instead of checking only one dInner, the check should loop over both
    // when both are positive the particle could be traveling from an NCD region to another one, missing the first NCD
    // and checking only one means missing the second intersection

    int nref;

    for(nref=0; nref<2; nref++){
      dtest = dInner[nref];
      if(dtest>fThreshold && dtest<dist){
        test = p + dtest * n;
        // JP changed this line. Should be using "test" - using "p" does not make sense
        z = test*fDirection>0?test.mag():-test.mag();
        // JP modified the skew calculation to include the fact that test is at the top of the NCD and fposition at bottom
        G4ThreeVector skew = (test - (fRmin*fDirection));  double radius=skew.mag();
        // JP changed this condition. It was requiring being on the surface of the cylinder instead of the top
        if(radius<fRadius+fThreshold &&fabs(z-fRmin)<fThreshold)dist=dtest;
      }
    }
    //now calculate cylindrical surface intersection:
    G4ThreeVector A = (p - fPosition); A = A - (A*fDirection)*fDirection;  //A is the projection of the vector from the NCD anchor center to p onto a plane perpendicular to the NCD.
    G4ThreeVector B = n - (n*fDirection)*fDirection;  //B is the projection of the direction vector onto a plane perpendicular to the NCD.

    // double a, b, c, q, d;  // parameters for the quadratic equation- ad**2+bd+c=0; d  is the test distance, q is the discriminant.
    double a, b, c;  // parameters for the quadratic equation- ad**2+bd+c=0; d  is the test distance, q is the discriminant.   WJH mod

    c = A.mag2() - fRadius*fRadius; b = 2 * A*B; a = B.mag2();

    // 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;
        z=test.mag();
        if (z>fRmin&&z<fRmax)dist = d1;
      }
      if (d2>fThreshold && d2<dist){
        test = p + d2*n;
        z=test.mag();
        if (z>fRmin&&z<fRmax)dist = d2;
      }
    }

    return dist;
}

G4double SNONCDAnchor::DistanceToIn(const G4ThreeVector &p)const{
    double z = p*fDirection;
    G4ThreeVector skew = (p - fPosition); skew = skew - (skew*fDirection)*fDirection;  //vector perpendicular to cylinder axis, through p.
    double radius = skew.mag();
    double dr = fRadius-radius;
    double dist = z-fRmax;
    if (dr > 0){  //if dr<0, just return dist
        double dz = z-fRmin;
        if (dist<0)dist = dz < dr ? dz : dr;
        else dist = sqrt(dr*dr + dist*dist);
    }
    if(dist<0) return 0;
    else return dist;
}


G4double SNONCDAnchor::DistanceToOut(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{
    double dist= DistanceToIn(p, n, dInner, dOuter);
    return dist;
}

G4double SNONCDAnchor::DistanceToOut(const G4ThreeVector &p)const {
    double z = p*fDirection;
    G4ThreeVector skew = (p - fPosition); skew = skew - (skew*fDirection)*fDirection;  //vector perpendicular to cylinder axis, through p.
    double radius = skew.mag();
    double dr = radius - fRadius;
    double dist=fRmin - z;;
    if (dr > 0){  //if dr<0, just return dist
        double dz = fRmax - z;
        if (dist<0)dist = dz < dr ? dz : dr;
        else {
            double d = sqrt(dr*dr + dz*dz);
            dist = dz < d ? dz : d;
        }
    }
    return dist;

}


G4bool SNONCDAnchor::OverlapsRegion(const G4ThreeVector corners[4])const {
    int i;
    double minang=-2;
    for(i=0;i<4;i++){
        minang=std::max(minang,corners[i].unit()*fDirection);
    }
    if(minang>cos((acos(fRmin/fRmax)+fRadius/fRmax)*1.05)){
        return true;
    }
    return false;
}