/*
 * SNORopeNet.cc
 *
 *  Created on: May 2, 2014
 *      Author: Aksel Hallin
 */

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

using namespace RAT;

SNORopeNet::SNORopeNet(DBLinkPtr geoDimensionsTable, G4double phi){
    //
    // phi is the the angle, in radians, of the floor anchor points that correspond to this net leg.
    // Each rope net object contains the entire rope from the equator, above the av missing the neck
    // and then back to the equator on the opposite side (phi2 = phi+0.9*pi).
    // The perpendicular vectors to the rope sides up to the spreader rope are f1Vertical and f2Vertical
    // At the spreader rope (4033.4mm) the rope follows a path to miss the neck
    // The perpendicular vector to this plane is fZaxis
    // The f1Cut and f2Cut values are the planes at which the rope changes definition. That's the spreader rope position.
    // Note that f1Vertical, f2Vertical and fZaxis are roughly parallel and positive in the right hand sense
    // as you go from the first to second leg

    f1Vertical=G4ThreeVector(sin(phi), -cos(phi),0);
    double phi2=phi+0.9*CLHEP::pi;
    f2Vertical=G4ThreeVector(-sin(phi2), cos(phi2),0);

    G4double fOuterAVradius = geoDimensionsTable->GetD( "inner_av_radius" ) +  geoDimensionsTable->GetD( "av_thickness" );
    G4double fDownRopeRadius = geoDimensionsTable->GetD( "down_rope_radius" );
    G4double fDownRopeFlatRadius = geoDimensionsTable->GetD( "down_rope_flatradius" );

    fZSpreader= geoDimensionsTable->GetD("spreader_rope_topz");
    fRmin=fOuterAVradius;
    fRmax=fOuterAVradius+2*fDownRopeFlatRadius;

    double theta=acos(fZSpreader/fRmin);
    fXaxis.setRThetaPhi(1,theta,phi);
    G4ThreeVector p2; p2.setRThetaPhi(1,theta,phi2);//
    fZaxis=fXaxis.cross(p2); fZaxis=fZaxis.unit();
    fYaxis=fZaxis.cross(fXaxis);

    // fZ1/2 are the radius of the rope. Used in transformed coordinate system.
    fZ1=-1.*fDownRopeRadius; fZ2=fDownRopeRadius;  //rope is 30 mm wide;
    fOverlapDistance=sqrt(fRmax*fRmax-fRmin*fRmin)+fZ2+5;  //horizontal distance required for rope to be checked.
    f1Cut=(fZaxis-f1Vertical).unit();  if(f1Cut.z()<0)f1Cut=-f1Cut;
    f2Cut=(fZaxis-f2Vertical).unit();  if(f2Cut.z()<0)f2Cut=-f2Cut;
    fThreshold = G4GeometryTolerance::GetInstance()->GetSurfaceTolerance();
}

G4double SNORopeNet::DistanceToIn(const G4ThreeVector &p, const G4ThreeVector &n, const G4double dInner[2], const G4double dOuter[2])const{
    // This routine enters with a proposed distance being the distance to the spherical radius.
    // if there are no intersections, returns kInfinity.
    double dtest=kInfinity;

    if(dInner[0]>fThreshold)dtest=dInner[0];
    else dtest=dInner[1];

    G4ThreeVector test=p+n*dtest;
    double retval=kInfinity;
    double dz;
    // Check the spherical surfaces at rmin
    if(dtest>fThreshold && fabs(test.mag()-fRmin)<fThreshold&&test.z()>0){
        double x=test*fXaxis;
        if(x>0 && test*f1Cut<0)dz=f1Vertical*test;
        else
          if(x<0 &&test*f2Cut<0)dz=f2Vertical *test;
          else dz=fZaxis *test;
        if(fZ1<dz&& dz<fZ2)retval=dtest;
    }
    if(dOuter[0]>fThreshold)dtest=dOuter[0];
    else dtest=dOuter[1];

    // Check the spherical surfaces at rmax
    if(dtest<retval&&dtest>fThreshold){
        test=p+n*dtest;
        if(fabs(test.mag()-fRmax)<fThreshold&&test.z()>0){
            double x=test*fXaxis;
            if(x>0 && test*f1Cut<0)dz=f1Vertical*test;
            else
              if(x<0 &&test*f2Cut<0)dz=f2Vertical *test;
              else dz=fZaxis *test;
            if(fZ1<dz&& dz<fZ2)retval=dtest;
        }
    }

    //Now calculate the distances to the "sides" of the ropes.
    // First the segments from the equator to spreader rope
    double vn=n*f1Vertical; double pn=p*f1Vertical;
    double dn;
    if(vn!=0){
        dn=(fZ1-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f1Cut<0&&test.mag()>fRmin &&test.mag()<fRmax&&test*fXaxis>0&&test.z()>0)retval=dn;
        }
        dn=(fZ2-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f1Cut<0 &&test.mag()>fRmin &&test.mag()<fRmax&&test*fXaxis>0&&test.z()>0)retval=dn;
        }
    }
    vn=n*f2Vertical; pn=p*f2Vertical;
    if(vn!=0){
        dn=(fZ1-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f2Cut<0&&test.mag()>fRmin &&test.mag()<fRmax&&test*fXaxis<0&&test.z()>0)retval=dn;
        }
        dn=(fZ2-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f2Cut<0&&test.mag()>fRmin &&test.mag()<fRmax&&test*fXaxis<0&&test.z()>0)retval=dn;
        }
    }
    // Then the great circle across the top of the AV missing the neck
    vn=n*fZaxis; pn=p*fZaxis;
    if(vn!=0){
        dn=(fZ1-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f1Cut>0 &&test*f2Cut>0&&test.mag()>fRmin &&test.mag()<fRmax&&test.z()>0)retval=dn;
        }
        dn=(fZ2-pn)/vn;
        if(dn<retval &&dn>fThreshold){
            test=p+dn*n;
            if(test*f1Cut>0 &&test*f2Cut>0&&test.mag()>fRmin &&test.mag()<fRmax&&test.z()>0)retval=dn;
        }
    }

    return retval;

}


G4double SNORopeNet::DistanceToIn(const G4ThreeVector &p)const{
    double retval;
    double r1= fabs(p.mag()-fRmax);  //radial distance to the top of the rope,
    double r2=fabs(p.mag()-fRmin);  //radial distance to the bottom of the rope
    if(r2<r1)r1=r2;
    if (r1>100)retval=r1; //don't calculate if far away from rope.
    else{
        double z1= fabs(p*fZaxis);       //  tangential distance to the edge o f the rope (at the top of the vessel
        double z2; if(p*fXaxis>0)z2= fabs(p*f1Vertical);
        else z2=fabs(p*f2Vertical);  //tangential distance to the vertical rope
        if(z1>z2)z1=z2;  // work with closest rope
        z1=z1-fZ2;
        if(z1<0)retval=r1;  //"inside" rope
        else
          if(z1>r2)retval=r2;
          else
            if(r1<0) retval=z1;
            else retval=sqrt(z1*z1+r1*r1);
    }

    return retval;
}

G4double SNORopeNet::DistanceToOut(const G4ThreeVector &p)const{
    double retval;
    double r1= fRmax-p.mag();  //radial distance to the av surface,
    double r2= p.mag()-fRmin;
    if(r1>r2)r1=r2;
    if (r1>100)retval=r1; //don't calculate if far away from surface.
    else{
        double z1= fabs(p*fZaxis);       //  tangential distance to the edge of the rope (at the top of the vessel
        double z2; if(p*fXaxis>0)z2= fabs(p*f1Vertical);
        else z2=fabs(p*f2Vertical);  //tangential distance to the vertical rope
        if(z1>z2)z1=z2;  // work with closest rope
        z1=z1-fZ2;
        if(z1>0)retval=r1;  //"outside" rope
        else
          if(z1>r2)retval=r2;
          else
            if(r1<0) retval=z1;
            else retval=sqrt(z1*z1+r1*r1);
    }
    return retval;
}

EInside SNORopeNet::Inside(const G4ThreeVector &p)const{
    EInside retval;
    if(p.mag()>fRmax+fThreshold || p.mag()<fRmin-fThreshold)retval=kOutside;
    else {
        double z;
        if(p*f1Cut>0 &&p*f2Cut>0) z=p*fZaxis;
        else
          if(p*fXaxis>0 &&p*f1Cut<0) z=p*f1Vertical;
          else z=p*f2Vertical;
        if (z>fZ2+fThreshold ||z<fZ1-fThreshold){  //outside the "sides of the rope"
            retval=kOutside;
        }else{  //between the "sides" of the rope
          if((fabs(p.mag()-fRmin)<fThreshold)||fabs(p.mag()-fRmax)<fThreshold ||fabs(z-fZ2)<fThreshold || fabs(z-fZ1)<fThreshold)retval=kSurface;
          else retval=kInside;
        }
    }
    return retval;
}


G4bool SNORopeNet::OverlapsRegion(const G4ThreeVector corners[4])const{
    //Returns true if the rope can be seen from any of the 4 corners. passes "between" the 4 corners;
    // If all four corners are on the same side of the rope, it returns false
    // This is designed to work in conjunction with the relatively small angular regions of the vessel;
    // it should not be used for general purposes.
    int i;
    for(i=0;i<4;i++){
        if(corners[i]*f1Cut>0 &&corners[i]*f2Cut>0){  //  on loop of rope
            if(fabs(corners[i]*fZaxis)<fOverlapDistance)return true;
        }else
          if(corners[i].z()>=0){
            if (corners[i]*fXaxis>0){if(fabs(corners[i]*f1Vertical)<fOverlapDistance)return true;}
            else
              if (fabs(corners[i]*f2Vertical)<fOverlapDistance)return true;
          }  // we don't calculate z for regions below the equator
    }
    return false;
}

G4ThreeVector SNORopeNet::SurfaceNormal(const G4ThreeVector &p, G4bool &isValid)const {
    G4ThreeVector retval;
    if(p.mag()>fRmax+fThreshold)isValid=false;
    else
      if(p.mag()<fRmin-fThreshold)isValid=false;
      else {
        double z=0;
        const G4ThreeVector *n;  //pointer to a constant- not a constant pointer!
        if(p*f1Cut>0 && p*f2Cut>0) n=&fZaxis;
        else
          if(p*fXaxis>0&&p*f1Cut<0) n=&f1Vertical;
          else n=&f2Vertical;
         z=p*(*n);
        if (z>fZ2+fThreshold ||z<fZ1-fThreshold)isValid=false;  //outside the "sides of the rope"
        else{  //between the "sides" of the rope
            isValid=true;
            if(fabs(p.mag()-fRmax)<fThreshold)retval=p.unit();
            else
              if (fabs(z-fZ2)<fThreshold)retval=n->unit(); // JP changed the sign
              else
                if (fabs(z-fZ1)<fThreshold)retval=-n->unit(); // JP changed the sign
                else
                  if (fabs(p.mag()-fRmin)<fThreshold)retval=-p.unit();
                  else isValid=false;
        }
      }
    return retval;
}