#include <RAT/QuadraticRoots.hh>
#include <RAT/SNOSV.hh>
#include <RAT/SNOSVBellyPlate.hh>
#include <RAT/SNONCDAnchor.hh>
#include <RAT/SphericalSubregionList.hh>
#include <CLHEP/Units/PhysicalConstants.h>
#include <stdio.h>
#include <RAT/DB.hh>
#include <RAT/Log.hh>

using namespace RAT;


SNOSV::SNOSV(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 fNcdTops = fInnerAVradius - geoDimensionsTable->GetD( "ncd_height" );

    // Initialize the region
    // InitializeSubregion(fNcdTops, fInnerAVradius,kPhysicalOuter);

    // Define if BellyPlates and NCD anchors will be used
    iAddBellyPlates = geoDimensionsTable->GetI("add_belly_plates");
    iAddNCDanchors = geoDimensionsTable->GetI("add_ncd_anchors");


    // Load and calculate dimensions of elements necessary for this geometry
    G4double fNeckBossThickness = geoDimensionsTable->GetD("neckboss_thickness");
    G4double fNeckBossCurveVert = geoDimensionsTable->GetD( "neckboss_curve_vertical");
    G4double fNeckBossCurveHor = geoDimensionsTable->GetD( "neckboss_curve_horizontal");
    G4double fNeckBossHeight = geoDimensionsTable->GetD("neckboss_height");
    G4double fNeckTop = geoDimensionsTable->GetD("neck_top_z");
    G4double fBPInnerSideMin = geoDimensionsTable->GetD( "bellyplate_inner_side_min");
    G4double fBPInnerSideMax = geoDimensionsTable->GetD( "bellyplate_inner_side_max");
    G4double fBellyPlateInnerRadius = geoDimensionsTable->GetD("bellyplate_inner_radius");
    G4double fNcdRadius = geoDimensionsTable->GetD("ncd_radius");
    fNeckBossInnerRadius = geoDimensionsTable->GetD("neckboss_inner_radius");
    fNeckInnerRadius = geoDimensionsTable->GetD("neck_inner_radius");
    fNeckBossOuterRadius = fNeckBossInnerRadius + fNeckBossThickness;
    fNeckRadius = fNeckBossOuterRadius + fThreshold; // geoDimensionsTable->GetD("neck_tolerance"); // Radius of neck+neck boss protruding the vessel

    // Defining geometry regions
    fNeckZ1 = sqrt(fInnerAVradius*fInnerAVradius-pow(fNeckBossOuterRadius+fNeckBossCurveHor,2))-fNeckBossCurveVert;
    fNeckZ2 = fNeckZ1 + fNeckBossHeight;
    fNeckZ3 = fNeckTop;

    // Initialize the region
    InitializeSubregion(fNcdTops, fInnerAVradius, fNeckZ1, kPhysicalOuter);



    int i;

    fRadii[0] = fInnerAVradius;
    fRadii[1] = fBellyPlateInnerRadius;
    fRadii[2] = fNcdTops;
    fRadii[3] = fInnerAVradius + geoDimensionsTable->GetD("sv_inner_tolerance"); // Added some tolerance

    std::list<SphericalSubregion *>list;

    // Declaring the belly plate regions
    if (iAddBellyPlates == 1)
      {
        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 SNOSVBellyPlate(fRadii[1], fRadii[0], xAxis, yAxis, zAxis, fBPInnerSideMin, fBPInnerSideMax);
          list.push_front(fBellyPlates[i]);
        }
      }

    if (iAddNCDanchors == 1)
      {
        // Declaring the NCD anchors region
        double x, y;
        for (i = 0, x = -5500; x <= 5500; x = x + 1000)for (y = -5500; y <= 5500; y = y + 1000){
            if (sqrt(x*x + y*y)>5525)continue;  // no ncds in the corners
            double z = sqrt(fRadii[0] * fRadii[0] - x*x - y*y);
            fNCDAnchors[i] = new SNONCDAnchor(fRadii[2], fRadii[0], fNcdRadius, G4ThreeVector(x, y, -z));
            list.push_front(fNCDAnchors[i]);
            i++;
          }
      }
    AddRegions(list);

}

G4double SNOSV::DistanceToNext(const G4ThreeVector &p, const G4ThreeVector &v, NextMode aNextMode)const{
    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;
        G4ThreeVector next;
        if(v.z()!=0)d=(fNeckZ1-p.z())/v.z();
        else d=kInfinity;
        if(p.z()>fNeckZ1-fThreshold || (d>fThreshold&&d<dist) || (d==0 && v.z()>0)){
          // Test Z1 intersections
          if(d>fThreshold&&d<dist){
            next=p+d*v;
            if(next.rho()>fNeckBossInnerRadius &&next.rho()<fNeckBossOuterRadius) dist=d;
          }

          // Only test if v.z() != 0
          if(v.z()!=0){
            // Now test Z2 intersections
            d=(fNeckZ2-p.z())/v.z();
            if(d>fThreshold&&d<dist){
              next=p+d*v;
              if (next.rho()>fNeckBossInnerRadius && next.rho()<fNeckInnerRadius)dist=d;
            }
            // Now test Z3 intersections
            d=(fNeckZ3-p.z())/v.z();
            if(d>fThreshold&&d<dist){
              next=p+d*v;
              if (next.rho()<fNeckInnerRadius)dist=d;
            }
          }
          // now go through the cylindrical intersections
          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);  //outside surface of the neckboss

          // WJH mod
          double d1, d2, dtemp;
          dtemp = dist;
          if( QuadraticRoots( a, 2.*b, c, d1, d2) ){
            if(d1>d2) { double temp=d1; d1=d2; d2=temp;}
            if (d1>fThreshold && d1<dtemp){
              next = p + d1*v;
              if (next.z()>=fNeckZ1 && next.mag()<=fRadii[0])dtemp=d1;
            }
            if (d2>fThreshold && d2<dtemp){
              next = p + d2*v;
              if (next.z()>=fNeckZ1 && next.mag()<=fRadii[0])dtemp=d2;
            }
            dist = dtemp;
          }  //q=0 or q negative mean no real intersection...

          c=(p.perp2()-fNeckBossInnerRadius*fNeckBossInnerRadius);
          // 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()>=fNeckZ1 && next.z()<=fNeckZ2)dist=d1;
            }
            if (d2>fThreshold && d2<dist){
              next = p + d2*v;
              if (next.z()>=fNeckZ1 && next.z()<=fNeckZ2)dist=d2;
            }
          }  //q=0 or q negative mean no real intersection...

          c=(p.perp2()-fNeckInnerRadius*fNeckInnerRadius);
          // 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;
            }
          }  //q=0 or q negative mean no real intersection...
        }
    }
    return dist;
}

EInside SNOSV::Inside(const G4ThreeVector& p) const{
    EInside in=SphericalSubregionList::Inside(p);
    //Now check to see if there are possible neck-intersections

    if(in==kOutside &&p.rho()<fNeckBossOuterRadius+fThreshold&&p.z()<fNeckZ3+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()>fNeckZ3-fThreshold&&p.rho()<fNeckInnerRadius){
            in=kSurface;
          }else
            if(p.rho()<fNeckBossInnerRadius-fThreshold){
              in=kInside;
            }else
              if(p.rho()<fNeckBossInnerRadius+fThreshold && p.z()>fNeckZ1 && p.z()<fNeckZ2){
                in=kSurface;
              }else
                if(p.z()>fNeckZ2+fThreshold){
                  if(p.rho()<fNeckInnerRadius-fThreshold)in=kInside;
                  else
                    if(p.rho()<fNeckInnerRadius+fThreshold)in=kSurface;
                }else
                  if(p.z()>fNeckZ2-fThreshold&&p.rho()<fNeckInnerRadius && p.rho()>fNeckBossInnerRadius){
                    in=kSurface;
                  }else
                    if(p.z()>fNeckZ1-fThreshold &&p.z()<fNeckZ1+fThreshold){
                      in=kSurface;
                    }
    }
    // JP mod - this case was nested inside the other one but they are mutually exclusive
    // because of the fNeckBossOuterRadius condition - should only apply in the protrusion of the neck into the SV
    if(p.mag()<fRadii[0] &&p.rho()>fNeckBossOuterRadius-fThreshold && p.rho()<fNeckBossOuterRadius+fThreshold){
      in=kSurface;
    }

    return in;
}

G4ThreeVector SNOSV::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const{
    G4ThreeVector norm=SphericalSubregionList::SurfaceNormal(p,isValid);
    //Now check to see if there are possible neck-intersections
    if(!isValid &&p.rho()<fNeckBossOuterRadius+fThreshold){
        if ((fabs(p.z()-fNeckZ3)<fThreshold&&p.rho()<fNeckInnerRadius)||
            (fabs(p.z()-fNeckZ1)<fThreshold &&p.rho()>fNeckBossInnerRadius)){
            isValid=true; norm.set(0,0,1);
        }else
          if(fabs(p.z()-fNeckZ2)<fThreshold &&p.rho()>fNeckBossInnerRadius&&p.rho()<fNeckInnerRadius ){
            isValid=true; norm.set(0,0,-1);
          }else
            if((fabs(p.rho()-fNeckBossInnerRadius)<fThreshold && p.z()>fNeckZ1 && p.z()<fNeckZ2)  ||
               (fabs(p.rho()-fNeckInnerRadius)<fThreshold && p.z()>fNeckZ2 && p.z()<fNeckZ3)){
              isValid=true; norm=p; norm.setZ(0); norm=norm.unit();
            }else
              if(fabs(p.rho()-fNeckBossOuterRadius)<fThreshold&&p.z()>fNeckZ1&&p.mag()<fRadii[0]){
                isValid=true; norm=p; norm.setZ(0); norm=-norm.unit();
              }
    }
    if(fabs(norm.mag()-1)>0.1){
      debug << "NativeGeometry: SNOSV Normal invalid\n";
    }
    return norm;
}



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

G4bool SNOSV::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[3];
    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=zMax;
        break;
      default:
        break;
    }
    pMin-=kCarTolerance;
    pMax+=kCarTolerance;

    return true;
  }

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

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

  return os;
}

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

G4VisExtent SNOSV::GetExtent() const
{
  double r=fRadii[3];
  return G4VisExtent(-r, r,-r, r,-r, r );
}

#include "G4VGraphicsScene.hh"

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

#include "G4Polyhedron.hh"

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

#include "Randomize.hh"
using namespace CLHEP;
G4ThreeVector SNOSV::GetPointOnSurface()const{
    double phi=RandFlat::shoot(0.0, CLHEP::twopi);
    double ct=RandFlat::shoot(-1,1);
    double st=sqrt(1-ct*ct);
    double r=fRadii[3];
    return G4ThreeVector(r*st*cos(phi),r*st*sin(phi),r*ct);
}