#include <G4GeometryTolerance.hh>
#include <RAT/SphericalSubregionList.hh>
#include <RAT/SNOAVBellyPlate.hh>
#include <RAT/DB.hh>

using namespace RAT;

SNOAVBellyPlate::SNOAVBellyPlate(DBLinkPtr geoDimensionsTable, 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 - fZOuter*fZOuter);
    fRhomax = sqrt(fRmax*fRmax - fZInner*fZInner);
    fRhoSlope = (fRhomax - fRhomin) / (fZInner - fZOuter); //so Rho=(fRhoSlope * (z-fZOffset));   Rhoslope is negative for the outside of the belly plates
    fZOffset = fZInner - fRhomax / fRhoSlope;  //fZOffset is positive


    G4double fInnerAVradius = geoDimensionsTable->GetD( "inner_av_radius" );
    G4double fAVthickness = geoDimensionsTable->GetD( "av_thickness" );
    G4double fGrooveInnerRadius = geoDimensionsTable->GetD("ropegroove_inner_radius");
    fGrooveRadius = geoDimensionsTable->GetD("ropegroove_radius");
    fGrooveHalfWidth = fGrooveRadius - fGrooveInnerRadius;

    fSlotHalfWidth   = geoDimensionsTable->GetD("ropeslot_half_width");
    fSlotRadius      = fGrooveRadius - geoDimensionsTable->GetD("groove_tolerance");
    fGrooveY1        = sqrt(pow(fInnerAVradius+fAVthickness/2.,2)-pow(fGrooveRadius,2));
    fGrooveY2        = fGrooveY1+geoDimensionsTable->GetD("ropegroove_depth");

}


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

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

EInside SNOAVBellyPlate::Inside(const G4ThreeVector &p)const {
    EInside in;
    double x = fabs(p*fXaxis);  //The panel is symmetric about x; the fabs allows easier testing of the rope groove
    double z = p*fZaxis; double y=p*fYaxis;
    double r = Radius(x); double r2 = Radius(z);
    if (r2<r)r = r2;  // A point on the thick region of belly plate has maximum r, which tapers down to the nominal r.
        // The smaller of the radius in the x or z corresponds to the larger distance from the panel center, which determines the actual radius.
    double delta = p.mag() - r; //distance from the surface to the point
    if (delta< -fThreshold)in = kInside;
    else if (delta>fThreshold)in = kOutside;
    else in = kSurface;

    if(in!=kOutside){  // for points on surface or inside, check to see if the point is in the rope groove.
        double rl; //distance from "center" of panel- so the rope groove itself is defined by rl=constant.
        if(z<0)rl=sqrt(x*x+z*z);else rl=x;  //  Since the rope is vertical above the equator, we can treat points above the equator as if they were on the equator
        if(fabs(rl-fGrooveRadius)<(fGrooveHalfWidth+fThreshold)){  //point is within the "U" defined by the outer surface of wider rope groove
            if(in==kSurface  && y<fGrooveY2 &&y>fGrooveY1)in=kOutside;   //in the cutaway region where the rope groove comes through the top of the panel
            else if(fabs(rl-fGrooveRadius)>(fGrooveHalfWidth-fThreshold)){  //point is in the surface layer of the "U" defined by the outer surface of wider rope groove
                if(y>fGrooveY1-fThreshold && y<fGrooveY2+fThreshold)in=kSurface;  //groove surface layer
                else in=kInside;
            }else if(fabs(rl-fSlotRadius)<(fSlotHalfWidth-fThreshold)){  //point is within the "U" defined by the slot
                  // if y<fGrooveY1, the point is inside the bulk of the panel or on the surface of the bevel- just keep the old in value;
                if(y>fGrooveY1+fThreshold)in=kOutside; //water volume
                else if(y>fGrooveY1-fThreshold) in=kSurface;
            }else if(fabs(rl-fSlotRadius)<(fSlotHalfWidth+fThreshold)){  //point is within the surface layer defined by the slot
                if(y>fGrooveY1+fThreshold  &&y<fGrooveY2-fThreshold)in=kOutside; //water volume
                else if(y>fGrooveY1-fThreshold) in=kSurface;  //surface includes tiny ribbon at bottom of groove, and edges of the slot
            }else{
                if(y>fGrooveY2+fThreshold)in=kInside;  //inside the bulk of the panel
                else if(y>fGrooveY1+fThreshold  &&y<fGrooveY2-fThreshold)in=kOutside; //water volume
                else if(y>fGrooveY1-fThreshold)in=kSurface;
            }

        }
    }
    return in;
}

G4ThreeVector SNOAVBellyPlate::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 y=p*fYaxis; double z = p*fZaxis;
    double rx = Radius(x); double rz = Radius(z);
    double r = rx<rz ? rx : rz; // always pick the minimum radius to deal with the curvature in both x and z directions.
    double delta = p.mag() - r;
    if(delta>fThreshold){
        isValid=false;  //outside the bellyplate
    }else{
        if(r<fRmax-fThreshold &&fabs(r-fRmin)>fThreshold){  //on a conical surface
            if (rx<rz){ //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));
                double phi = atan2(z, y);
                if (x>0){
                    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/  We implicitly assume zaxis is vertical here.
                double phi = p.phi();
                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();
                }
            }
        }

        // check for the rope groove
        double rl;
        if(z<0)rl=sqrt(x*x+z*z);else rl=fabs(x);
        if(fabs(rl-fGrooveRadius)<(fGrooveHalfWidth+fThreshold)){  //point is within the "U" defined by the outer surface of wider rope groove
            if((rl-fGrooveRadius)>(fGrooveHalfWidth-fThreshold)){  //point is in the bottom surface layer of the "U" defined by the outer surface of wider rope groove
                if(y>fGrooveY1+fThreshold && y<fGrooveY2-fThreshold){
                    if(z>0)normal=-x/fabs(x)*fXaxis; else if (x!=0) normal=-(fZaxis-z/x*fXaxis).unit();else normal=-fZaxis;
                }else isValid=false;
            }else if(-(rl-fGrooveRadius)>(fGrooveHalfWidth-fThreshold)){  //point is in the top surface layer of the "U" defined by the outer surface of wider rope groove
                    if(y>fGrooveY1+fThreshold && y<fGrooveY2-fThreshold){
                        if(z>0)normal=-x/fabs(x)*fXaxis; else if (x!=0) normal=(fZaxis-z/x*fXaxis).unit();else normal=fZaxis;
                    }else isValid=false;
            }else if(fabs(rl-fSlotRadius)<(fSlotHalfWidth-fThreshold)){  //point is within the "U" defined by the slot
                if(y>fGrooveY1+fThreshold)isValid=false; //water volume
                else if(y>fGrooveY1-fThreshold)normal=fYaxis;
            }else if(fabs(rl-fSlotRadius)<(fSlotHalfWidth+fThreshold)){  //point is within the surface layer defined by the slot
                if(y>fGrooveY1+fThreshold  &&y<fGrooveY2-fThreshold)isValid=false; //water volume
                else if(y>fGrooveY2-fThreshold){
                    if(z>0)normal=-x/fabs(x)*fXaxis; else if (x!=0) normal=(fZaxis-z/x*fXaxis).unit();else normal=fZaxis;
                }else{
                    normal=fYaxis;
                }
            }else{
                if(fabs(y-fGrooveY1)<fThreshold)normal=fYaxis;
                else if(fabs(y-fGrooveY2)<fThreshold)normal=-fYaxis;
                else if(delta<-fThreshold) isValid=false;
            }

        }
    }
    return normal;
}


G4double SNOAVBellyPlate::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{
    // find the intersections with the surface of the belly plate. qualifiedDistance is the shortest previously found distance.
    //Check the intersections with the inner radius
    //In the interest of computational efficiency, we cheat in one instance.  The rope grooves are part of the outside of the AV, but actually extend inside to
    // almost the ID of the AV.  Thus, if the qualifiedDistance brings us to the AV "normal" outer radius, we "unqualify" it for the rope grooves.

    G4ThreeVector test;  //temporary variable to hold a possible intersection.  That intersection gets checked before an intersection is valid
    int i;
    double x,z,rloop; //coordinates of intersection wrt to the center of a panel, along the horizontal (x), vertical (z) and radial (rloop) directions.
    double dtest;  // possible distance- as we check candidate distances, we find the smallest dtest that is further away from our current position(p) than
    // our tolerance (fThreshold).
    if(dInner[0]>fThreshold)dtest=dInner[0];
    else dtest=dInner[1];
    double dist=kInfinity;

    if (dtest>fThreshold){  //check intersections with inner radius
            test = p + dtest * n;
            x = fabs(test*fXaxis); z = (test*fZaxis);
            if (z<0) rloop=sqrt(x*x+z*z); else rloop=x;
            double rr = test.mag();
            if (fabs(rr-fRmin)<fThreshold&&(x>fZOuter ||fabs(z)>fZOuter))dist=dtest;
    }

    if(dOuter[0]>fThreshold)dtest=dOuter[0];
    else dtest=dOuter[1];
    if(dtest>fThreshold && dtest<dist){
        test = p + dtest * n;
        x = fabs(test*fXaxis); z = (test*fZaxis);
        if (z<0) rloop=sqrt(x*x+z*z); else rloop=x;
        double rr = test.mag();
        if ((fabs(rr - fRmax)<fThreshold && x<fZInner &&fabs(z) <fZInner &&fabs(rloop-fSlotRadius)>fSlotHalfWidth)||
                ((fabs(rloop-fGrooveRadius)<fGrooveHalfWidth)&&(test*fYaxis<fGrooveY2)))dist=dtest;
    }
    //calculate conical intersections : first Z-cones since this doesn't require any rotation
    //solve the conical equation-- (p+d*n).X()**2+(p+d*n).Y()**2=(((p+d*n).Z()-fZOffset)*fRhoSlope)**2
    // Express this as a quadratic equation ax**2+bx+c=0 and solve
    double a, b, c, q, d;
    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();
        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;
                if (Inside(test) == kSurface)
                    dist = d;
            }
            d = (-b - q) / (2 * a);
            if (d>fThreshold && d<dist){
                test = p + d*n;
                if (Inside(test) == kSurface)
                    dist = d;
            }
        }
    }
    // 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();
        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;
                if (Inside(test) == kSurface)
                    dist = d;
            }
            d = (-b - q) / (2 * a);
            if (d>fThreshold && d<dist){
                test = p + d*n;
                if (Inside(test) == kSurface)
                    dist = d;
            }
        }
    }

    //now check rope groove surfaces
    px=G4ThreeVector(p*fXaxis, p*fYaxis,p*fZaxis);
    nx=G4ThreeVector(n*fXaxis, n*fYaxis,n*fZaxis);
    if(nx.y()!=0){
        d=(fGrooveY1-px.y())/nx.y();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(Inside(test)==kSurface)dist=d;
        }
        d=(fGrooveY2-px.y())/nx.y();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
    }
    if(nx.x()!=0){
        d=(fGrooveRadius+fGrooveHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(fGrooveRadius-fGrooveHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(-fGrooveRadius+fGrooveHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(-fGrooveRadius-fGrooveHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(fSlotRadius+fSlotHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(fSlotRadius-fSlotHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(-fSlotRadius+fSlotHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }
        d=(-fSlotRadius-fSlotHalfWidth-px.x())/nx.x();
        if(d>fThreshold&&d<dist){
            test=p+d*n;
            if(test.z()>fThreshold&&Inside(test)==kSurface)dist=d;
        }

    }

    //now do cylindrical surfaces

    b = 2 * (px.x()*nx.x() + px.z()*nx.z());
    double c0 = px.x()*px.x() + px.z()*px.z();
    a = nx.x()*nx.x()+nx.z()*nx.z();
    double cc[4];

    cc[0]=c0-pow(fGrooveRadius+fGrooveHalfWidth,2);
    cc[1]=c0-pow(fGrooveRadius-fGrooveHalfWidth,2);
    cc[2]=c0-pow(fSlotRadius+fSlotHalfWidth,2);
    cc[3]=c0-pow(fSlotRadius-fSlotHalfWidth,2);


    for(i=0;i<4;i++){
        q = b*b - 4 * a*cc[i];
        if (q >= 0){
            q = sqrt(q);
            d = (-b + q) / (2 * a);
            if (d>fThreshold && d<dist){
                test = p + d*n;
                if (test.z()<0 && Inside(test) == kSurface) dist = d;
            }
            d = (-b - q) / (2 * a);
            if (d>fThreshold && d<dist){
                test = p + d*n;
                if (test.z()<0 && Inside(test) == kSurface) dist = d;
            }
        }
    }
    return dist;
}


G4bool SNOAVBellyPlate::OverlapsRegion(const G4ThreeVector corners[4])const {
    double ctheta=cos(sin(fZOuter*sqrt(2.0)/fRmin)+acos(fRmin/fRmax));
    int i;
    bool retval=false;
    for(i=0;!retval && i<4;i++){
        retval=((corners[i].unit()*fYaxis)>ctheta);
    }
    return retval;
}