////////////////////////////////////////////////////////////////////////
///
/// \class
///
/// \brief
///
/// \author Aksel Hallin  aksel.hallin@ualberta.ca
///
/// REVISION HISTORY:\n
///     Jul 9, 2014 :  Aksel Hallin  \n
///
/// \detail
////
////////////////////////////////////////////////////////////////////////

#include <RAT/QuadraticRoots.hh>
#include <RAT/SNORopeLoop.hh>
#include <G4GeometryTolerance.hh>
#include <RAT/SphericalSubregionList.hh>
#include <RAT/DB.hh>

using namespace RAT;

// A SNORopeLoop is the portion of the loop inside the rope groove of the belly plates- that is, a semicircular torus.
SNORopeLoop::SNORopeLoop(DBLinkPtr geoDimensionsTable, G4double aPhi):fXaxis(sin(aPhi),-cos(aPhi),0),fYaxis(cos(aPhi),sin(aPhi),0),fZaxis(0,0,1){

  G4double fInnerAVradius = geoDimensionsTable->GetD( "inner_av_radius" );
  G4double fAVthickness = geoDimensionsTable->GetD( "av_thickness" );
  G4double fUpRopeRadius   = geoDimensionsTable->GetD( "up_rope_radius" );
  //G4double fSNORopeRadius = geoDimensionsTable->GetD("sno_rope_radius");
  //G4double fBellyPlateInnerRadius = geoDimensionsTable->GetD("bellyplate_inner_radius");
  fGrooveInnerRadius = geoDimensionsTable->GetD("ropegroove_inner_radius");
  fGrooveOuterRadius = geoDimensionsTable->GetD("ropegroove_outer_radius");
  fOuterAVradius = fInnerAVradius + fAVthickness;

  //G4double fBPthickness = geoDimensionsTable->GetD( "bellyplate_additional_thickness")*2 + fAVthickness;

  fLoopRadius=fGrooveInnerRadius + fUpRopeRadius + 2*geoDimensionsTable->GetD("groove_tolerance");

  // Rho distance to the axis of the rope. Same definition as for the groove.
  // This is the exact position, but a tweaked one is used
  // Tweaked position. If changed, make sure SNORope and SNOAVBellyPlate agree


  fMaxHalfThick=fUpRopeRadius;// Half thickness of rope going out from the center of the panel
  fMinHalfThick=fUpRopeRadius*3.1416/4.;// Thickness of flattened rope. Rectangular shape. One side is 2r, the other is redefined.
  fRadius = fInnerAVradius+2*geoDimensionsTable->GetD("groove_tolerance")+
    geoDimensionsTable->GetD("ropegroove_depth")/2.;

  fOrigin=fRadius*fYaxis;// Actual vector corresponding to the center of the rope loop


  fThreshold = G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();  //Geant's tolerance to determine if one is on a surface or not.

    }

EInside SNORopeLoop::Inside(const G4ThreeVector &p)const{
    EInside in;
    // JP - Any point above 0 is outside, as it corresponds to the regular rope
    if(p.z()>0) return kOutside;
    // The rope is assumed to be shaped like a rectangular box against the groove.
    double x = p*fXaxis;
    double y = p*fYaxis;
    double z = p*fZaxis;

    double delta = fabs(sqrt(x*x + z*z)-fLoopRadius);
    double deltay = fabs(y - fRadius);
    // JP rewrote all of these conditions
    if(delta>fMinHalfThick+fThreshold  || deltay>fMaxHalfThick+fThreshold) in=kOutside;
    else
      if (delta<fMinHalfThick-fThreshold && deltay<fMaxHalfThick-fThreshold) in=kInside;
      else in=kSurface;

    return in;
}


G4double SNORopeLoop::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double *, const G4double *)const{
    G4ThreeVector relpos=p-fOrigin;
    G4ThreeVector local(relpos*fXaxis, relpos*fYaxis, relpos*fZaxis);
    G4ThreeVector nl(n*fXaxis, n*fYaxis, n*fZaxis);
    G4ThreeVector test;
    double dist=kInfinity;
    double d;
    // First check the flat surfaces at fixed y positions - looks fine
    if(nl.y()!=0){
        d=(-fMaxHalfThick-local.y())/nl.y();
        if(d>0&&d<dist){
            test=p+n*d;
            if(Inside(test)==kSurface)dist=d;
        }
        d=(fMaxHalfThick-local.y())/nl.y();
        if(d>0&&d<dist){
            test=p+n*d;
            if(Inside(test)==kSurface)dist=d;
        }
    }
//  This routine calculates intersections with an arbitrary radius.  Use it to find the Rope intersection, for instance...
    // next is filled with the position of the next positive distance intersection.
    //    double a,b,c,r2,q,d;
    double a,b,c,r2;          // WJH mod
    b = (local.x()*nl.x()+local.z()*nl.z());  //note we drop the factor of 2 here and in the solution formula
    a = nl.x()*nl.x()+nl.z()*nl.z();
    r2=local.x()*local.x()+local.z()*local.z();
    double radius=fLoopRadius-fMinHalfThick;
    c=(r2-radius*radius);

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


    radius=fLoopRadius+fMinHalfThick;   /// A SNORopeLoop is the portion of the loop inside the rope groove of the belly plates- that is, a semicircular torus.

    c=(r2-radius*radius);

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


    return dist;
}


G4double SNORopeLoop::DistanceToIn(const G4ThreeVector &p)const {
    G4ThreeVector relpos=p-fOrigin;
    G4ThreeVector local(relpos*fXaxis, relpos*fYaxis, relpos*fZaxis);
    double d,d2;
    if(local.y()>0)d=local.y()-fMaxHalfThick;
    else d=local.y()+fMaxHalfThick;
    if(d>0)d2=d*d;
    else d2=0;   /// A SNORopeLoop is the portion of the loop inside the rope groove of the belly plates- that is, a semicircular torus.

    if(local.z()>0)d2+=local.z()*local.z();
    else{
        double del=sqrt(local.x()*local.x()+local.z()*local.z())-fLoopRadius;
        if(del>fMinHalfThick)d2+=pow(del-fMinHalfThick,2);
        else
          if(del<-fMinHalfThick)d2+=pow(del+fMinHalfThick,2);
    }
    return sqrt(d2);
}

G4double SNORopeLoop::DistanceToOut(const G4ThreeVector &p)const {
    G4ThreeVector relpos=p-fOrigin;
    G4ThreeVector local(relpos*fXaxis, relpos*fYaxis, relpos*fZaxis);
    double d;
    if(local.y()>0)d=fMaxHalfThick-local.y();
    else d=local.y()+fMaxHalfThick;


    double del=sqrt(local.x()*local.x()+local.z()*local.z())-fLoopRadius;
    if(del>0)del=fMinHalfThick-del;
    else del+=fMinHalfThick;
    if(d<del)return d;
    else return del;
}

G4ThreeVector SNORopeLoop::SurfaceNormal(const G4ThreeVector &p)const{
    bool isvalid;
    return SurfaceNormal(p,isvalid);
}

G4ThreeVector SNORopeLoop::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
  // JP rewrote all of SurfaceNormal - similar issues as Inside function
  G4ThreeVector n;
  double x = p*fXaxis;
  double y = p*fYaxis;
  double z = p*fZaxis;

  // Loop & y deltas
  double delta = sqrt(x*x + z*z)-fLoopRadius;
  double deltay = y - fRadius;

  isValid=true;


  // Flat surfaces first
  if(fabs(deltay-fMaxHalfThick) < fThreshold) n = fYaxis;
  else
    if(fabs(fMaxHalfThick+deltay) < fThreshold) n = -fYaxis;
  // Curved surfaces follow
    else
      if(fabs(delta-fMinHalfThick) < fThreshold){
        // n should point down and out
        if(x!=0) n=-(fZaxis+x/z*fXaxis).unit();
        else n=-fZaxis;
      }
      else
        if(fabs(fMinHalfThick+delta) < fThreshold){
          // n should point in and up
          if(x!=0) n=(fZaxis+x/z*fXaxis).unit();
          else n = fZaxis;
        }
  else isValid=false;


    return n;
}

G4bool SNORopeLoop::OverlapsRegion(const G4ThreeVector corners[4])const{
    int i;
    double dmax=-1e8; double dmin=1e8; // dmin and dmax will be the smallest and largest angles between the corners of a region and the
    // vector from the center of the AV to the center of the ropeloop.
    for(i=0;i<4;i++){
        double t=(corners[i]*fYaxis/corners[i].mag());
        if(fabs(t)<1)t=acos(t);
        else
          if(t<-1)t=acos(-1);
          else
            if(t>1)t=acos(1);
        if(t<dmin)dmin=t;
        if(t>dmax)dmax=t;
    }
    return (dmin<fGrooveOuterRadius/fOuterAVradius  && dmax>fGrooveInnerRadius/fOuterAVradius);
    // dmin/dmax are angles

}