#include <RAT/QuadraticRoots.hh>
#include <RAT/SNOAV.hh>
#include <RAT/SphericalSubregionList.hh>
#include <RAT/SphericalSubregion.hh>
#include <RAT/SNOAVBellyPlate.hh>
#include <RAT/SNONCDAnchor.hh>
#include <RAT/SNORopeNet.hh>
#include <CLHEP/Units/PhysicalConstants.h>
#include <stdio.h>
#include <RAT/DB.hh>

using namespace RAT;


SNOAV::SNOAV(const G4String &aTitle):SphericalSubregionList(aTitle){

    // Load the database
    RAT::DBLinkPtr geoDimensionsTable = RAT::DB::Get()->GetLink( "NATIVE_GEO_DIMENSIONS", "natgeo_dimensions" );
    G4double fInnerAVradius = geoDimensionsTable->GetD( "inner_av_radius" );
    G4double fAVthickness = geoDimensionsTable->GetD( "av_thickness" );
    G4double fOuterAVradius = fInnerAVradius + fAVthickness;
    G4double fBellyPlateThickness = geoDimensionsTable->GetD( "bellyplate_additional_thickness" );
    G4double fBellyPlateOuterRadius = fOuterAVradius + fBellyPlateThickness;

    // Initialize the region
    InitializeSubregion(fOuterAVradius,fBellyPlateOuterRadius, fOuterAVradius, kPhysicalInner);

    // Define if BellyPlates will be used
    iAddBellyPlates = geoDimensionsTable->GetI("add_belly_plates");

    // Load and calculate dimensions of elements necessary for this geometry
    G4double fNeckBossInnerRadius = geoDimensionsTable->GetD("neckboss_inner_radius");
    G4double fNeckBossThickness = geoDimensionsTable->GetD("neckboss_thickness");
    G4double fNeckBossCurveVert = geoDimensionsTable->GetD( "neckboss_curve_vertical");
    G4double fNeckBossCurveHor = geoDimensionsTable->GetD( "neckboss_curve_horizontal");
    G4double fNeckInnerRadius = geoDimensionsTable->GetD("neck_inner_radius");
    G4double fNeckThickness = geoDimensionsTable->GetD("neck_thickness");
    G4double fNeckBossFeature = geoDimensionsTable->GetD("neckboss_feature");
    G4double fNeckTop = geoDimensionsTable->GetD("neck_top_z");
    fNeckBossOuterRadius = fNeckBossInnerRadius + fNeckBossThickness;

    // Defining geometry regions
    fRadii[0] = fOuterAVradius;
    fRadii[1] = fBellyPlateOuterRadius;

    fNeckRadius          = fNeckBossOuterRadius + fThreshold; //geoDimensionsTable->GetD("neck_tolerance");
    fNeckOuterRadius     = fNeckInnerRadius + fNeckThickness;
    fSphereMaxZ          = sqrt(fOuterAVradius*fOuterAVradius- fNeckRadius*fNeckRadius);


    G4double fBPOuterSideMin = geoDimensionsTable->GetD( "bellyplate_outer_side_min");
    G4double fBPOuterSideMax = geoDimensionsTable->GetD( "bellyplate_outer_side_max");


    G4double fNeckZ1_SV = sqrt(fInnerAVradius*fInnerAVradius-pow(fNeckBossOuterRadius+fNeckBossCurveHor,2))-fNeckBossCurveVert;

    fNeckZ1              = sqrt(fOuterAVradius*fOuterAVradius-fNeckBossOuterRadius*fNeckBossOuterRadius);
    fNeckZ2              = fNeckZ1_SV+fNeckBossFeature;
    fNeckZ3              = fNeckTop;

    std::list<SphericalSubregion *>list;
    if (iAddBellyPlates == 1)
      {
        int i;
        G4ThreeVector zAxis(0, 0, 1);
        G4ThreeVector xAxis, yAxis;
        for (i = 0; i < 10; i++){
          xAxis.setRThetaPhi(1, CLHEP::halfpi, (i-2.5)*CLHEP::pi/5);
          yAxis.setRThetaPhi(1, CLHEP::halfpi, i *CLHEP::pi/5);
          fBellyPlates[i]= new SNOAVBellyPlate(geoDimensionsTable, fRadii[0],fRadii[1],xAxis,yAxis,zAxis,fBPOuterSideMin,fBPOuterSideMax);
          list.push_front(fBellyPlates[i]);
        }
      }
    AddRegions(list);

}

G4double SNOAV::DistanceToNext(const G4ThreeVector &p, const G4ThreeVector &v, NextMode aNextMode)const{
    // start position is p, direction is v;  DistanceToNext solves p+v*distance=next intersection with a boundary
    G4double dist;

    dist=SphericalSubregionList::DistanceToNext(p,v, aNextMode);
    //Now check to see if there are possible neck-intersections
    if((v.y()*p.x()-v.x()*p.y())/v.rho()<fNeckBossOuterRadius+fThreshold){  //does the trajectory come within the radius of the neck?
        G4double d=(v.z()!=0)?(fNeckZ1-p.z())/v.z():kInfinity;
        if(p.z()>fNeckZ1 || (d>fThreshold&&d<dist)){   //only need to check these cases for neck intersections
            G4ThreeVector next;
            if(v.z()!=0){
                d=(fNeckZ2-p.z())/v.z();
                if(d>fThreshold&&d<dist){next=p+d*v; if (next.rho()>fNeckOuterRadius && next.rho()<fNeckBossOuterRadius)dist=d;}
                d=(fNeckZ3-p.z())/v.z();
                if(d>fThreshold&&d<dist){next=p+d*v; if (next.rho()<fNeckOuterRadius)dist=d;}
            }
            // now go through the cylindrical intersections-- solve the quadratic equation ax**2+bx+c=0; in this case (p+d*v)**2=cylindrical radius **2.
            G4double b = p.x()*v.x()+p.y()*v.y();  //note we drop the factor of 2 here and in the solution formula
            G4double a = v.x()*v.x()+v.y()*v.y();
            G4double c=(p.perp2()-fNeckBossOuterRadius*fNeckBossOuterRadius);

            // 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>fThreshold && d1<dist){
                next=p+d1*v;
                if (next.z()>=fNeckZ1 && next.z()<=fNeckZ2)dist=d1;
              }
              if (d2>fThreshold && d2<dist){
                next=p+d2*v;
                if (next.z()>=fNeckZ1 && next.z()<=fNeckZ2)dist=d2;
              }
            }

            c=(p.perp2()-fNeckOuterRadius*fNeckOuterRadius);

            // WJH mod
            if( QuadraticRoots( a, 2.*b, c, d1, d2) ){
              if(d1>d2) { double temp=d1; d1=d2; d2=temp;}
              if (d1>fThreshold && d1<dist){
                next=p+d1*v;
                if (next.z()>=fNeckZ2 && next.z()<=fNeckZ3)dist=d1;
              }
              if (d2>fThreshold && d2<dist){
                next=p+d2*v;
                if (next.z()>=fNeckZ2 && next.z()<=fNeckZ3)dist=d2;
              }
            }
        }
    }
    return dist;
}

EInside SNOAV::Inside(const G4ThreeVector &p)const{
    // If the point is not obviously in the AV, check all regions individually
    EInside in=SphericalSubregionList::Inside(p);
    //Now check to see if there are possible neck-intersections
    if(in==kOutside &&p.rho()<fNeckBossOuterRadius+fThreshold && p.z()>0.){  //does the trajectory come within the radius of the neck?
        if(p.z()<fNeckZ1-fThreshold){
            in=kInside;
        }else
          if(p.z()<fNeckZ2-fThreshold){
            if(p.rho()<fNeckBossOuterRadius-fThreshold)in=kInside;
            else
              if(p.rho()>fNeckBossOuterRadius+fThreshold)in=kOutside;
              else in=kSurface;
        } else
            if(p.z()>fNeckZ2+fThreshold && p.z()<fNeckZ3-fThreshold){
              if(p.rho()<fNeckOuterRadius-fThreshold)in=kInside;
              else
                if(p.rho()>fNeckOuterRadius+fThreshold)in=kOutside;
                else in=kSurface;
        } else
              if(p.z()>fNeckZ3+fThreshold){
                in=kOutside;
        } else
                if(p.z()>fNeckZ3-fThreshold &&p.rho()<fNeckOuterRadius+fThreshold){
                  in=kSurface;
                } else
                  if(p.z()>fNeckZ2-fThreshold&&p.z()<fNeckZ2+fThreshold&&p.rho()>fNeckOuterRadius){
                    in=kSurface;
                  }else{
                    //we should never get here!  This is for setting a debugger trip.
                    in=kOutside;
                  }
    }
    return in;
}

G4ThreeVector SNOAV::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
    G4ThreeVector norm;
    norm=SphericalSubregionList::SurfaceNormal(p,isValid);
    //Now check to see if there are possible neck-intersections
    if(!isValid &&p.rho()<fNeckBossOuterRadius+fThreshold){
        if(p.z()>fNeckZ1-fThreshold && p.z()<fNeckZ2-fThreshold){
            if(fabs(p.rho()-fNeckBossOuterRadius)<fThreshold){
                isValid=true;
                norm=p; norm.setZ(0.0); norm=norm.unit();
            }
        } else
          if(p.z()>fNeckZ2+fThreshold && p.z()<fNeckZ3-fThreshold){
            if(fabs(p.rho()-fNeckOuterRadius)<fThreshold){
              isValid=true;
              norm=p; norm.setZ(0.0); norm=norm.unit();
            }
        } else
            if(fabs(p.z()-fNeckZ3)<fThreshold &&p.rho()<fNeckOuterRadius+fThreshold){
              isValid=true; norm=G4ThreeVector(0,0,1);
            } else
              if(fabs(p.z()-fNeckZ2)<fThreshold&&p.rho()>fNeckOuterRadius){
                isValid=true; norm=G4ThreeVector(0,0,1);
              }else{
                //we should never get here!  This is for setting a debugger trip.
                isValid=false;
                norm=G4ThreeVector(0,0,0);
              }
    }
    return norm;
}



#include <G4AffineTransform.hh>
#include <G4VoxelLimits.hh>
G4bool SNOAV::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=fRadii[1];
    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-rmax;
    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+fNeckZ3;
    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=fNeckZ3;
        break;
      default:
        break;
    }
    pMin-=kCarTolerance;
    pMax+=kCarTolerance;

    return true;
  }

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

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

  return os;
}

/////////////////////////////////////////////////////////////////////////////
//
// Methods for visualisation
#include <G4VisExtent.hh>

G4VisExtent SNOAV::GetExtent() const
{
  double r=fRadii[1]+30;  //ensure the extent is a little bigger than the radius.
  return G4VisExtent(-r, r,-r, r,-r, r );
}

#include <G4VGraphicsScene.hh>

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

#include <G4Polyhedron.hh>

G4Polyhedron* SNOAV::CreatePolyhedron () const
{
  return new G4PolyhedronSphere (0, 0,0, CLHEP::twopi, 0,CLHEP::pi);
}

#include <Randomize.hh>
using namespace CLHEP;
G4ThreeVector SNOAV::GetPointOnSurface()const{
    double twopi=CLHEP::twopi;
    double phi=RandFlat::shoot(0.0, twopi);
    double ct=RandFlat::shoot(-1,1);  //ct means cos(theta)- throw it flat from -1 to 1
    double st=sqrt(1-ct*ct);   // sin(theta)
    double r=fRadii[1];
    return G4ThreeVector(r*st*cos(phi),r*st*sin(phi),r*ct);
}