#include <G4GeometryTolerance.hh>
#include <RAT/SphericalSubregionList.hh>
#include <RAT/SNOAVBellyPlate.hh>
#include <RAT/DB.hh>
#include <RAT/QuadraticRoots.hh>
#include <CLHEP/Units/PhysicalConstants.h>


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" );
    fYmin = fInnerAVradius*cos(CLHEP::pi/10.); // Using half the angular distance between belly plates
    //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");
    //G4double fBellyPlateInnerRadius = geoDimensionsTable->GetD("bellyplate_inner_radius");
    //G4double fBPthickness = geoDimensionsTable->GetD( "bellyplate_additional_thickness")*2+fAVthickness;
    //G4double fGrooveDepth = geoDimensionsTable->GetD("ropegroove_depth");
    //G4double fSNORopeRadius = geoDimensionsTable->GetD("sno_rope_radius");
    // This definition is the exact position of the groove plane according to drawings
    // It can be slightly smaller than the innerAV, which is a very large problem in the solid definition
    // Using a tweaked definition (different by <5mm) to avoid redefining volumes. Propagate the change to the ropes as well
    fGrooveY1 = fInnerAVradius+2*geoDimensionsTable->GetD("groove_tolerance");
    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 && delta<-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;

    G4double outerDelta = fabs(p.mag()-fRmax);
    G4double innerDelta = fabs(p.mag()-fRmin);

    double rl;
    // Radial distance (or x) to the point being tested. Used in mutliple places below.
    if(z<0)rl=sqrt(x*x+z*z);
    else rl=fabs(x);



    // Order of checks
    // 1. Outer radius (beware of the slot hole)
    // 2. AV radius, outside the belly plate area
    // 3. Conical sections
    // 4. Rope groove, only for points within its x-z radius
    if(delta>fThreshold){
        isValid=false;  //outside the bellyplate
    }else{
      // Check the outer-most spherical face
      if( outerDelta<fThreshold ){
        if(fabs(z)<fZInner && fabs(x)<fZInner && (rl-fSlotRadius)>(fSlotHalfWidth-fThreshold))normal=p.unit();
        else isValid=false;
      // Now the inner face, actually outside the belly plate (regular sphere surface)
      }else
        if(innerDelta<fThreshold){
          if(fabs(z)>fZOuter && fabs(x)>fZOuter)normal=p.unit();
          else isValid=false;
      // Now check the rope groove surfaces (recall that the opening of the slot was already taken care of)
      // Only do this check if the point is in the rope groove region
      } else
          if(fabs(rl-fGrooveRadius)<(fGrooveHalfWidth+100*fThreshold)){
            // Start by setting validity to false. Only change it if a normal is found
            isValid=false;
            // Check first the flat surfaces at fGrooveY1
            if(fabs(y-fGrooveY1)<fThreshold){normal=fYaxis; isValid=true;}
            // Check the other surface, where the slot matters
            if(fabs(y-fGrooveY2)<fThreshold && fabs(rl-fSlotRadius)>(fSlotHalfWidth-fThreshold)){normal=-fYaxis; isValid=true;}
            // Now test the more complicated side faces of the U (there are 4 of them, 2 for the groove and 2 for the slot)
            // Check for outer U surface of the groove OR the outer U surface of the slot
            // rl value needs to be at +/- threshold of the radius+halfwidth
            if((((rl-fGrooveRadius)>(fGrooveHalfWidth-fThreshold))&& y>(fGrooveY1+fThreshold) && y<(fGrooveY2-fThreshold)) ||
               (((rl-fSlotRadius)>(fSlotHalfWidth-fThreshold))&& (rl-fSlotRadius)<(fSlotHalfWidth+fThreshold) &&  y>(fGrooveY2-fThreshold))){
              if(z>0)normal=-x/fabs(x)*fXaxis;
              else
                if (x!=0) normal=(fZaxis+x/z*fXaxis).unit();
                else normal=fZaxis;
              isValid=true;
              // Now check the inner U surface for the groove OR the slot, same calculation for both
            }else
              if(((-(rl-fGrooveRadius)>(fGrooveHalfWidth-fThreshold))&& y>(fGrooveY1+fThreshold) && y<(fGrooveY2-fThreshold)) ||
                 ((-(rl-fSlotRadius)>(fSlotHalfWidth-fThreshold))&& -(rl-fSlotRadius)<(fSlotHalfWidth+fThreshold) && y>(fGrooveY2-fThreshold))){
                if(z>0)normal=x/fabs(x)*fXaxis;
                else
                  if (x!=0) normal=-(fZaxis+x/z*fXaxis).unit();
                  else normal=-fZaxis;
                isValid=true;
              }
          }
    }

    // Now check the conical surfaces.
    // JP- the cutaway region at the top of the rope groove is complicated.
    // Enter check if the point is in the conical region AND the norm hasn't been set before by the groove surfaces
    if(normal.mag()<fThreshold && r<fRmax-fThreshold &&fabs(r-fRmin)>fThreshold &&
       ((fabs(x)>fZInner-fThreshold && fabs(x)<fZOuter+fThreshold) ||
        (fabs(z)>fZInner-fThreshold && fabs(z)<fZOuter+fThreshold))){
      // Entering because point is on a conical surface
      isValid=true;
      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();
        }
      }
    }
    return normal;
}

G4double SNOAVBellyPlate::CalculatePlaneIntersection(const G4ThreeVector &aPosition, const G4ThreeVector &aDirection, const G4ThreeVector &aPlaneNormal, const G4double &aRadius)const{
  G4ThreeVector planePoint  = -aPlaneNormal*aRadius;

  G4double normalDotDir = aPlaneNormal.dot(aDirection);
  if (normalDotDir == 0) return -1.; // Negative distances are never used
  else return -(aPlaneNormal.dot(aPosition) - aPlaneNormal.dot(planePoint))/normalDotDir;

}


G4double SNOAVBellyPlate::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{

    G4ThreeVector test;  // Temporary variable to hold a possible intersection. It gets checked before declaring it valid.
    int i;
    double x,z,rloop; // Intersection wrt panel center along the horizontal (x), vertical (z) and radial (rloop) directions.
    double dtest;  // Possible distance- check/find smallest dtest that is further away from our current position(p) than
    // our tolerance (fThreshold).
    double dist=kInfinity;

    // Exploring both dInner crossings (in case photon travels out of the bp and leaves somewhere outside)
    int nref;

    // Establish and validate distance to the "inner" belly plate face (really is the outerAV surface)
    for(nref=0; nref<2; nref++){
      dtest = dInner[nref];
      if (dtest>fThreshold && dtest<dist){
        test = p + dtest * n;
        // Making sure "test" is in this belly plate
        if(test*fYaxis>fYmin){
          x = (test*fXaxis); z = (test*fZaxis);
          double rr = test.mag();
          // Validating that position is at the boundary and not outside the belly plate sides
          if (fabs(rr-fRmin)<fThreshold&&(fabs(x)>fZOuter ||fabs(z)>fZOuter))dist=dtest;
        }
      }
    }

    // Defining which dOuter to use (quadratic equation, 2 solutios, get positive one)
    if(dOuter[0]>fThreshold)dtest=dOuter[0];
    else dtest=dOuter[1];
    // Establish and validate distance to outer belly plate. Extra validation because of rope groove & slot
    // Validation steps
    // 1. Point is on the surface and within the external boundaries of belly plate
    // 2. Point is outside the slot region (everywhere)
    // 3. Point is outside the exposed groove region (where curvature makes fGrooveY1== bellyplate rho)
    if(dtest>fThreshold && dtest<dist){
        test = p + dtest * n;
        if(test*fYaxis>fYmin){
          x = (test*fXaxis); z = (test*fZaxis);
          // rloop is either a radial position or x, to compare with the rope groove & slot
          if (z<0) rloop=sqrt(x*x+z*z);
          else rloop=fabs(x);
          double rr = test.mag();
          if (fabs(rr - fRmax)<fThreshold && fabs(x)<fZInner &&fabs(z) <fZInner){
            if (fabs(rloop-fSlotRadius)>fSlotHalfWidth){
              if (test*fYaxis>fGrooveY2) dist=dtest;
              else
                if(fabs(rloop-fGrooveRadius)>fGrooveHalfWidth) dist=dtest;
            }
          }
        }
    }


    // Intersection with the conical sections of the belly plates ("sides")
    // 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, d;       // WJH mod
    // 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();

        // WJH mod
        double d1, d2;
        if( QuadraticRoots( a, b, c, d2, d1) ){
          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, d2, d1) ){
          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 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&& test*fYaxis > fYmin)dist=d;
      }
      d=(fGrooveY2-px.y())/nx.y();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
    }
    if(nx.x()!=0){
      d=(fGrooveRadius+fGrooveHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(fGrooveRadius-fGrooveHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(-fGrooveRadius+fGrooveHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(-fGrooveRadius-fGrooveHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(fSlotRadius+fSlotHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(fSlotRadius-fSlotHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(-fSlotRadius+fSlotHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }
      d=(-fSlotRadius-fSlotHalfWidth-px.x())/nx.x();
      if(d>fThreshold&&d<dist){
        test=p+d*n;
        if(Inside(test)==kSurface&& test*fYaxis > fYmin)dist=d;
      }

    }


    // There are 2 cylindrical surfaces that come from the groove at z<0
    // Using Aksels code for this
    px=G4ThreeVector(p*fXaxis, p*fYaxis,p*fZaxis);
    nx=G4ThreeVector(n*fXaxis, n*fYaxis,n*fZaxis);

    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++){
      // WJH mod
      double d1, d2;
      if( QuadraticRoots( a, b, cc[i], d2, d1) ){
        if(d1<d2) { double temp=d1; d1=d2; d2=temp;}
        if (d1>fThreshold && d1<dist){
          test = p + d1*n;
          if (test.z()<0 && Inside(test) == kSurface&& test*fYaxis > fYmin) dist = d1;
        }
        if (d2>fThreshold && d2<dist){
          test = p + d2*n;
          if (test.z()<0 && Inside(test) == kSurface&& test*fYaxis > fYmin) dist = d2;
        }
      }

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