////////////////////////////////////////////////////////////////////////////////
///
/// SNORope.cc
/// Created on: Apr 27, 2014
///    Author: Aksel Hallin
///////////////////////////////////////////////////////////////////////////////

#include <RAT/RunManager.hh>
#include <RAT/Gsim.hh>
#include <RAT/Trajectory.hh>
#include <RAT/SNORope.hh>
#include <RAT/SNORopeNet.hh>
#include <RAT/SNORopeLoop.hh>
#include <RAT/SNOSpreaderRope.hh>
#include <RAT/SphericalSubregionList.hh>
#include <CLHEP/Units/PhysicalConstants.h>
#include <stdio.h>
#include <math.h>
#include <G4GeometryTolerance.hh>
#include <RAT/DB.hh>

using namespace RAT;


SNORope::SNORope(const G4String &aTitle): SphericalSubregionList(aTitle ){
    //6056.118 is the outer radius of the AV and 6084.058 is the outer radius of the rope net- they define the ropenet shell that we consider

    RAT::DBLinkPtr geoDimensionsTable = RAT::DB::Get()->GetLink( "NATIVE_GEO_DIMENSIONS", "natgeo_dimensions" );
    G4double fInnerAVradius = geoDimensionsTable->GetD( "inner_av_radius" );
    G4double fOuterAVradius = fInnerAVradius + geoDimensionsTable->GetD( "av_thickness" );
    G4double fDownRopeFlatRadius = geoDimensionsTable->GetD( "down_rope_flatradius" );
    fDownRopeRadius = geoDimensionsTable->GetD( "down_rope_radius" );

    InitializeSubregion(fOuterAVradius, fOuterAVradius+2*fDownRopeFlatRadius,kAbstract);


    G4double fAVthickness = geoDimensionsTable->GetD( "av_thickness" );
    G4double fSNORopeRadius = geoDimensionsTable->GetD("sno_rope_radius");
    fPSUPradius = geoDimensionsTable->GetD("psup_radius");
    fUpRopeRadius   = geoDimensionsTable->GetD( "up_rope_radius" );
    fAVRadius  = fOuterAVradius + geoDimensionsTable->GetD("rope_tolerance");
    fRopeCylinderRadius  = fInnerAVradius + fAVthickness/2. + fSNORopeRadius;


    fDownZMax=0.0;
    fUpZMin=0.0;
    fUpZMax=sqrt(fPSUPradius*fPSUPradius-fRopeCylinderRadius*fRopeCylinderRadius);
    fDownZMin=-sqrt(fPSUPradius*fPSUPradius-pow(fAVRadius+fDownRopeRadius,2));

    fPhiMapOffset=fDownRopeRadius/fRopeCylinderRadius*1.01*180.0/CLHEP::pi;
    int i,j;
    for(i=0;i<360;i++){
        fRopePosition[i]=NULL; fRopeZMin[i]=1e9; fRopeZMax[i]=-1e9; fRopeRadius[i]=0;
    }
    for(i=0;i<10;i++)for(j=0;j<2;j++){
        double phi=(i*36+4.725*2*(j-0.5));  //4.725 degrees between upward ropes and center of whiffle tree; 10 whiffle trees 36 degrees apart
        int n=floor(phi+fPhiMapOffset);  if(n<0)n+=360;
        fRopePosition[n]=new G4ThreeVector(); fRopePosition[n]->setRThetaPhi(fRopeCylinderRadius,CLHEP::halfpi,phi*CLHEP::pi/180);
        fRopeRadius[n]=fUpRopeRadius; fRopeZMax[n]=fUpZMax;
        fRopeZMin[n]=fUpZMin;
    }

    for(i=0;i<20;i++){
        double phi=9+i*18;   //downward ropes are 18 degrees apart, starting at 9 degrees.
        int n=floor(phi+fPhiMapOffset); //note fPhiMapOffset is a little more than half a rope radius angle- makes each bin have only one rope.
        if(n<0)n+=360;
        fRopePosition[n]=new G4ThreeVector(); fRopePosition[n]->setRThetaPhi(fAVRadius+fDownRopeRadius,CLHEP::halfpi,phi*CLHEP::pi/180);
        fRopeRadius[n]=fDownRopeRadius; fRopeZMax[n]=fDownZMax;
        fRopeZMin[n]=fDownZMin;
    }
    fRadii[0]=fRopeCylinderRadius-fUpRopeRadius;
    fRadii[1]=fAVRadius+2*fDownRopeRadius;

    fThreshold = G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();

    std::list<SphericalSubregion *>list;
    for(i=0;i<10;i++){fRopeLoop[i]=new SNORopeLoop(geoDimensionsTable, i*CLHEP::pi/5);list.push_front(fRopeLoop[i]);}
    for(i=0;i<10;i++){
        fRopeNet[i]=new SNORopeNet(geoDimensionsTable, CLHEP::pi*(27+i*36)/180.); list.push_front(fRopeNet[i]);
    }
    for(i=0;i<10;i++){
        fSpreaderRope[i]=new SNOSpreaderRope(geoDimensionsTable,CLHEP::pi*(9+i*36)/180.); list.push_front(fSpreaderRope[i]);
    }

    AddRegions(list);

    fRminNet=fRopeNet[0]->GetRmin();
    fRmaxNet=fRopeNet[0]->GetRmax();
}



void SNORope::CalculateCylindricalPoints(const G4ThreeVector &aPosition, const G4ThreeVector &aDirection, G4double distances[4][2],std::list<G4double> &list)const{
    double a,b,c,r2,q,d;  //solve quadratic equation ad**2+b*d +c=0; with discriminant q, to solve (aPosition +d*aDirection)**2=fRadii[ii]**2.
    int i;
    list.clear();
    b = (aPosition.x()*aDirection.x()+aPosition.y()*aDirection.y());  //note we drop the factor of 2 here and in the solution formula
    a = aDirection.x()*aDirection.x()+aDirection.y()*aDirection.y();
    r2=aPosition.x()*aPosition.x()+aPosition.y()*aPosition.y();

    for (i = 0; i<2; i++){
        c=(r2-fRadii[i]*fRadii[i]);
        q=b*b-a*c;
        if(q>0){
            q=sqrt(q);
            d=(-b+q)/a; distances[i][0]=d; if(d>0){list.push_front(d);}
            d=(-b-q)/a; distances[i][1]=d; if(d>0){list.push_front(d); }
        }else if(q==0){
            distances[i][0]=-b; distances[i][1]=-100; if(b<0){list.push_front(-b);}
        }else{
            distances[i][0]=-100; distances[i][1]=-100;
        }

    }
    list.sort();
}

void SNORope::CalculateCylindricalPoints(const G4ThreeVector &aPosition, const G4ThreeVector &aDirection, G4double aRadius, G4double &dist)const{
    double a,b,c,r2,q,d,d2;  //solve quadratic equation ad**2+b*d +c=0; with discriminant q, to solve (aPosition +d*aDirection)**2=fRadii[ii]**2.
//  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.
    b = (aPosition.x()*aDirection.x()+aPosition.y()*aDirection.y());  //note we drop the factor of 2 here and in the solution formula
    a = aDirection.x()*aDirection.x()+aDirection.y()*aDirection.y();
    r2=aPosition.x()*aPosition.x()+aPosition.y()*aPosition.y();
    d=1e8;
    c=(r2-aRadius*aRadius);
    q=b*b-a*c;
    if(q>0){
        q=sqrt(q);
        d=(-b+q)/a; if(d<0)d=1e8;
        d2=(-b-q)/a; if(d2>0 &&d2<d)d=d2;
    }  //q=0 or q negative mean no real intersection...
    dist=d;
}


EInside SNORope::Inside(const G4ThreeVector& p) const{
    EInside in;
    double zmin,zmax,radius;
    const G4ThreeVector *rope=RopePosition(p.getPhi(),zmin,zmax,radius);
    in=kOutside;
    if(rope!=NULL &&p.z()<zmax-fThreshold && p.z()>zmin+fThreshold){
        double rho=(p-*rope).rho();
        if(rho<radius-fThreshold)in=kInside;
        else if(rho<radius+fThreshold)in=kSurface;
    }

    if(in==kOutside)in=SphericalSubregionList::Inside(p);
    return in;
}

G4ThreeVector SNORope::SurfaceNormal(const G4ThreeVector& p, G4bool &isValid) const{
      // Returns the outwards pointing unit normal of the shape for the
      // surface closest to the point at offset p.
    double phi=p.getPhi();
    double zmin,zmax,radius;
    const G4ThreeVector *rope=RopePosition(phi,zmin,zmax,radius);
    G4ThreeVector n;
    if(rope!=NULL){
        n=p-*rope;
        n.setZ(0);
        if(fabs(n.mag()-radius)<10*fThreshold){
            isValid=true;
            return n.unit();  //increase threshold since the ropes are far and small.
        }
    }   /// Implements the G4VSolid function

    n=SphericalSubregionList::SurfaceNormal(p,isValid);  //only reaches here if no cylindrical normals are found.
    return n;
}

const  G4ThreeVector *SNORope::RopePosition(const G4double phi)const{
    int n=(int)floor(phi*360/CLHEP::twopi+fPhiMapOffset);  //fPhiMapOffset is a little more than the angle from the rope radius- makes each rope fit inside one bin
    if(n<0)n+=360; n=n%360;
    return fRopePosition[n];    /// Implements the G4VSolid function

}

const  G4ThreeVector *SNORope::RopePosition(const G4double phi, G4double &zmin, G4double &zmax, G4double &radius)const{
    int n=(int)floor(phi*360/CLHEP::twopi+fPhiMapOffset);  //fPhiMapOffset is a little more than the angle from the rope radius- makes each rope fit inside one bin
    if(n<0)n+=360; n=n%360;
    zmin=fRopeZMin[n]; zmax=fRopeZMax[n]; radius=fRopeRadius[n];
    return fRopePosition[n];
}


G4double SNORope::DistanceToIn(const G4ThreeVector& p, const G4ThreeVector& v) const{
      // Return the distance along the normalised vector v to the shape,
      // from the point at offset p. If there is no intersection, return
      // kInfinity. The first intersection resulting from `leaving' a
      // surface/volume is discarded. Hence, it is tolerant of points on
      // the surface of the shape.
    double dist;
    EInside in=Inside(p);
    if(in==kInside){  //should never happen  --
        return kInfinity;
    }else if(in==kSurface&&v*SurfaceNormal(p)<0){
        return 0;  //  on the border- Distance to In returns 0.
    }

    dist=DistanceToNext(p,v);
    return dist;
}


G4double SNORope::DistanceToIn(const G4ThreeVector& p)const{
    double dist=1e8;
    int k=(int)floor(p.phi()*360/CLHEP::twopi+fPhiMapOffset);
    int i;
    for(i=k-9;i<k+9;i++){
        int ii=i%360;  if(ii<0)ii+=360;
        if(fRopePosition[ii]!=NULL){
            double d=(*fRopePosition[ii]-p).rho()-fRopeRadius[ii];
            if(d<dist)dist=d;
        }
    }

      // Calculate the distance to the nearest surface of a shape from an
      // outside point. The distance can be an underestimate.
    double dist2=SphericalSubregionList::DistanceToIn(p);
    if(dist2<dist)dist=dist2;
    return dist;
}

G4double SNORope::DistanceToOut(const G4ThreeVector& p,
                   const G4ThreeVector& v,
                   const G4bool calcNorm,
                   G4bool *validNorm,
                   G4ThreeVector *norm)const {
      // Return the distance along the normalised vector v to the shape,
      // from a point at an offset p inside or on the surface of the shape.
      // Intersections with surfaces, when the point is < Tolerance/2 from a
      // surface must be ignored.
      // If calcNorm==true:
      //    validNorm set true if the solid lies entirely behind or on the
      //              exiting surface.
      //    n set to exiting outwards normal vector (undefined Magnitude).
      //    validNorm set to false if the solid does not lie entirely behind
      //              or on the exiting surface
      // If calcNorm==false:
      //    validNorm and n are unused.
      //
      // Must be called as solid.DistanceToOut(p,v) or by specifying all
      // the parameters.
    double dist=DistanceToNext(p,v);
    if(calcNorm){
        G4ThreeVector test=p+dist*v;
        *norm=SurfaceNormal(test);
        *validNorm=true;
    }
    return dist;
}

G4double SNORope::DistanceToNext(const G4ThreeVector &p, const G4ThreeVector &v)const{
    std::list<G4double> list;
    double distances[4][2];
    CalculateCylindricalPoints(p,v,distances,list);
    std::list<G4double>::iterator  aa;
    if(list.empty())return kInfinity;
    G4ThreeVector test;
    int i;
    double dist=kInfinity;
    int philist[4];
    G4ThreeVector next,n;
    n=v.cross(G4ThreeVector(0,0,1));n=n.unit();
    //Go through the list of distances.
    int iphi;
    if(p.rho()>=fRadii[0]&& p.rho()<=fRadii[1]){
        iphi=floor(p.phi()*360/CLHEP::twopi+fPhiMapOffset); i=1;
        philist[0]=iphi<0? (iphi+360)%360:iphi%360;
    }else i=0;
    for(aa=list.begin();aa!=list.end(); aa++){
        double checkdist=*aa;
        test=p+checkdist*v;
        iphi=floor(test.phi()*360/CLHEP::twopi+fPhiMapOffset);
        philist[i++]=iphi<0? (iphi+360)%360:iphi%360;
    }
    int step=(test*n)>0?1:-1;  //rho dot (v cross z) has the same sign as the rotation
    int k;
    if(philist[1]==philist[0]){philist[1]=(philist[0]+step)%360; if(philist[1]<0)philist[1]+=360;}
    for(k=philist[0];k!=philist[1];){
        if(k==356){
          test=p;

        }
        if(fRopePosition[k]!=NULL){
            test=p-*fRopePosition[k];
            if(fabs(test*n)<fRopeRadius[k]){  //intersection is possible
                double d;
                CalculateCylindricalPoints(test,v,fRopeRadius[k],d);
                next=p+d*v;
                if(next.z()>fRopeZMin[k]&&next.z()<fRopeZMax[k]&&d<dist)dist=d;
            }
        }
        k=(k+step)%360;if(k<0)k+=360;
    }
    if(i>2){
        if(philist[i-1]==philist[i-2]){philist[i-1]=(philist[i-2]+step)%360; if(philist[i-1]<0)philist[i-1]+=360;}
        for(k=philist[i-2];k!=philist[i-1];){
            if(fRopePosition[k]!=NULL){
                test=p-*fRopePosition[k];
                if(fabs(test*n)<fRopeRadius[k]){  //intersection is possible
                    double d;
                    CalculateCylindricalPoints(test,v,fRopeRadius[k],d);
                    next=p+d*v;
                    if(next.z()>fRopeZMin[k]&&next.z()<fRopeZMax[k]&&d<dist)dist=d;
                }
            }
            k=(k+step)%360;if(k<0)k+=360;
        }

    }

    double dist2=SphericalSubregionList::DistanceToNext(p,v);
    if(dist2<dist)dist=dist2;

    return dist;
}


G4double SNORope::DistanceToOut(const G4ThreeVector& p)const{
      // Calculate the distance to the nearest surface of a shape from an
      // inside point. The distance can be an underestimate.
    double zmin,zmax,radius,dist;
    const G4ThreeVector *rope=RopePosition(p.phi(),zmin,zmax,radius);
    if(rope !=NULL){
        G4ThreeVector test=p-*rope;
        dist=fabs(radius-test.rho());
        if(dist>zmax-test.z())dist=zmax-test.z();
        if(dist>test.z()-zmin)dist=test.z()-zmin;
    }else{  //debug
        dist=fabs(fRopeCylinderRadius-p.rho());
    }
    double dist2=SphericalSubregionList::DistanceToOut(p);
    if(dist2<dist)dist=dist2;
    return dist;
}


#include "G4AffineTransform.hh"
#include "G4VoxelLimits.hh"

G4bool SNORope::CalculateExtent( const EAxis pAxis,
                                  const G4VoxelLimits& pVoxelLimit,
                                  const G4AffineTransform& pTransform,
                                        G4double& pMin, G4double& pMax ) const
{
    // Taken directly from G4Sphere.cc....

    // Special case handling for solid spheres-shells
    // (rotation doesn't influence).
    // Compute x/y/z mins and maxs for bounding box respecting limits,
    // with early returns if outside limits. Then switch() on pAxis,
    // and compute exact x and y limit for x/y case
    // G4double rmax=8400.0;    // Make the value a little bigger than Radius of psup (where the rope stops).
    G4double rmax=fPSUPradius + 10.;  // Make the value a little bigger than Radius of psup (where the rope stops).
    G4double xoffset,xMin,xMax;
    G4double yoffset,yMin,yMax;
    G4double zoffset,zMin,zMax;

    G4double diff1,diff2,delta,maxDiff,newMin,newMax;
    G4double xoff1,xoff2,yoff1,yoff2;

    xoffset=pTransform.NetTranslation().x();
    xMin=xoffset;
    xMax=xoffset+rmax;
    if (pVoxelLimit.IsXLimited())
    {
      if ( (xMin>pVoxelLimit.GetMaxXExtent()+kCarTolerance)
        || (xMax<pVoxelLimit.GetMinXExtent()-kCarTolerance) )
      {
        return false;
      }
      else
      {
        if (xMin<pVoxelLimit.GetMinXExtent())
        {
          xMin=pVoxelLimit.GetMinXExtent();
        }
        if (xMax>pVoxelLimit.GetMaxXExtent())
        {
          xMax=pVoxelLimit.GetMaxXExtent();
        }
      }
    }
    yoffset=pTransform.NetTranslation().y();
    yMin=yoffset-rmax;
    yMax=yoffset+rmax;
    if (pVoxelLimit.IsYLimited())
    {
      if ( (yMin>pVoxelLimit.GetMaxYExtent()+kCarTolerance)
        || (yMax<pVoxelLimit.GetMinYExtent()-kCarTolerance) )
      {
        return false;
      }
      else
      {
        if (yMin<pVoxelLimit.GetMinYExtent())
        {
          yMin=pVoxelLimit.GetMinYExtent();
        }
        if (yMax>pVoxelLimit.GetMaxYExtent())
        {
          yMax=pVoxelLimit.GetMaxYExtent();
        }
      }
    }

    zoffset=pTransform.NetTranslation().z();
    zMin=zoffset-rmax;
    zMax=zoffset+rmax;
    if (pVoxelLimit.IsZLimited())
    {
      if ( (zMin>pVoxelLimit.GetMaxZExtent()+kCarTolerance)
        || (zMax<pVoxelLimit.GetMinZExtent()-kCarTolerance) )
      {
        return false;
      }
      else
      {
        if (zMin<pVoxelLimit.GetMinZExtent())
        {
          zMin=pVoxelLimit.GetMinZExtent();
        }
        if (zMax>pVoxelLimit.GetMaxZExtent())
        {
          zMax=pVoxelLimit.GetMaxZExtent();
        }
      }
    }

    // Known to cut sphere

    switch (pAxis)
    {
      case kXAxis:
        yoff1=yoffset-yMin;
        yoff2=yMax-yoffset;
        if ((yoff1>=0) && (yoff2>=0))
        {
          // Y limits cross max/min x => no change
          //
          pMin=xMin;
          pMax=xMax;
        }
        else
        {
          // Y limits don't cross max/min x => compute max delta x,
          // hence new mins/maxs
          //
          delta=rmax*rmax-yoff1*yoff1;
          diff1=(delta>0.) ? std::sqrt(delta) : 0.;
          delta=rmax*rmax-yoff2*yoff2;
          diff2=(delta>0.) ? std::sqrt(delta) : 0.;
          maxDiff=(diff1>diff2) ? diff1:diff2;
          newMin=xoffset-maxDiff;
          newMax=xoffset+maxDiff;
          pMin=(newMin<xMin) ? xMin : newMin;
          pMax=(newMax>xMax) ? xMax : newMax;
        }
        break;
      case kYAxis:
        xoff1=xoffset-xMin;
        xoff2=xMax-xoffset;
        if ((xoff1>=0) && (xoff2>=0))
        {
          // X limits cross max/min y => no change
          //
          pMin=yMin;
          pMax=yMax;
        }
        else
        {
          // X limits don't cross max/min y => compute max delta y,
          // hence new mins/maxs
          //
          delta=rmax*rmax-xoff1*xoff1;
          diff1=(delta>0.) ? std::sqrt(delta) : 0.;
          delta=rmax*rmax-xoff2*xoff2;
          diff2=(delta>0.) ? std::sqrt(delta) : 0.;
          maxDiff=(diff1>diff2) ? diff1:diff2;
          newMin=yoffset-maxDiff;
          newMax=yoffset+maxDiff;
          pMin=(newMin<yMin) ? yMin : newMin;
          pMax=(newMax>yMax) ? yMax : newMax;
        }
        break;
      case kZAxis:
        pMin=zMin;
        pMax=zMax;
        break;
      default:
        break;
    }
    pMin-=kCarTolerance;
    pMax+=kCarTolerance;

    return true;
  }

  G4GeometryType SNORope::GetEntityType(void)const{ return G4String("SNORope");}

  std::ostream& SNORope::StreamInfo( std::ostream& os ) const
{
  G4int oldprc = os.precision(16);
  os << "-----------------------------------------------------------\n"
     << "    *** Dump for solid - " << GetName() << " ***\n"
     << "    ===================================================\n"
     << " Solid type: SNORope\n"
     << " No variable parameters! \n"
     << "-----------------------------------------------------------\n";
  os.precision(oldprc);

  return os;
}

/////////////////////////////////////////////////////////////////////////////
//
// Methods for visualisation
#include "G4VisExtent.hh"

G4VisExtent SNORope::GetExtent() const
{
  double r=fRadii[1]+40;
  return G4VisExtent(-r, r,-r, r,-fPSUPradius, fPSUPradius );
}


#include "G4VGraphicsScene.hh"

void SNORope::DescribeYourselfTo ( G4VGraphicsScene& scene ) const
{
  scene.AddSolid (*this);
}

#include "G4Polyhedron.hh"


#include "Randomize.hh"
using namespace CLHEP;
G4ThreeVector SNORope::GetPointOnSurface()const{
    double downToUpAreaRatio=(fDownZMax-fDownZMin)/(fUpZMax-fUpZMin)*fDownRopeRadius/fUpRopeRadius;
    double upFraction=1/(1+downToUpAreaRatio);
    double phi=RandFlat::shoot(0.0, CLHEP::twopi);
    int i=(int)RandFlat::shoot(0.0,20.0);
    G4ThreeVector pos;
    if(RandFlat::shoot(0.0,1)<upFraction){
            pos=fUpRopePositions[i/2][i%2];
            pos.setZ(RandFlat::shoot(fUpZMin,fUpZMax));
    }else{
        pos=fDownRopePositions[i];
        pos.setZ(RandFlat::shoot(fDownZMin,fDownZMax));
    }
    pos.setPhi(phi);
    return pos;
}