#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 {

    EInside in;
    double radius = Radius(p); double z=p.mag();
    z=p.mag();
    if (radius<fRadius + fThreshold  &&p*fDirection>0){
        if (z>fRmin + fThreshold)in = kOutside;
        else if (z>fRmin - fThreshold)in = kSurface;
        else in = kInside;
    }
    else if (radius>fRadius + fThreshold){
        in = kOutside;
    }
    return in;
}

G4ThreeVector SNONCDAnchor::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
    G4ThreeVector skew = (p - fPosition); skew = 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*)const{
    double dist=kInfinity;
    G4ThreeVector test = n.cross(fDirection); test = test.unit();
    if (fabs(test*(p - fPosition))>fRadius)return dist;  // no intersection possible

    // check top of NCD Anchor.  The ncd ir (inner radius) must be the distance to which dInner is calculated.
    double dtest;
    if(dInner[0]>fThreshold)dtest=dInner[0]; else dtest=dInner[1];
    test = p + dtest * n;
    double z = test*fDirection>0?p.mag():-p.mag();
    G4ThreeVector skew = (test - fPosition);  double radius=skew.mag();
    if(fabs(fRadius-radius)<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.
    a = A.mag2() - fRadius*fRadius; b = 2 * A*B; c = B.mag2();
    q = b*b - 4 * a*c;
    if (q >= 0){
        q = sqrt(q);
        d = (-b + q) / (2 * a);
        if (d>fThreshold && d<dist){
            test = p + d*n;
            z=test.mag();
            if (z>fRmin&&z<fRmax)dist = d;
        }
        d = (-b - q) / (2 * a);
        if (d>fThreshold && d<dist){
            test = p + d*n;
            z=test.mag();
            if (z>fRmin&&z<fRmax)dist = d;
        }
    }
    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);
    }
    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;
}