/*
 * 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.
    // The rope net is defined from the equator, up to the spreader rope (at
    // 4033.4 mm above the equator and then opens up to follow a great circle across the AV (missing the neck) and down the other side.
    // All the rope net have phi differences of 0.9 pi- either between legs 0 and 2 or between 3 and 1.
    // These differ in the sense in which they deflect around the neck.  For the subnet with legs at 9, 27, 189, 207, we make two SNORopeNet objects-
    // the first leg from 27-189 degrees, and the second at 207
    f1Vertical=G4ThreeVector(sin(phi), -cos(phi),0);  // Vertical in this context is perpendicular to the rope.  Note that f1Vertical, f2Vertical and fZaxis
        // are all roughly parallel and positive in the right hand sense as you go from the first to second leg.
    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");   //Top of spreader rope-
    fRmin=fOuterAVradius + geoDimensionsTable->GetD("rope_tolerance");
    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=-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;
    if(dInner[0]>fThreshold)dtest=dInner[0]; else dtest=dInner[1];
    G4ThreeVector test=p+n*dtest;
    double retval=kInfinity;
    double dz;
    if(dtest>fThreshold && fabs(test.mag()-fRmin)<fThreshold&&test.z()>0){ //check if the test point is on one of the spherical surfaces of the rope
        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];

    if(dtest<retval&&dtest>fThreshold){
        test=p+n*dtest;
        if(fabs(test.mag()-fRmax)<fThreshold&&test.z()>0){ //check if the test point is on one of the spherical surfaces of the rope
            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
    double vn=n*f1Vertical; double pn=p*f1Vertical;
    double dn;
    if(vn!=0){
        dn=(fZ1-pn)/vn;
        if(dn<retval &&dn>0){
            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>0){
            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>0){
            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>0){
            test=p+dn*n;
            if(test*f2Cut<0&&test.mag()>fRmin &&test.mag()<fRmax&&test*fXaxis<0&&test.z()>0)retval=dn;
        }
    }
    vn=n*fZaxis; pn=p*fZaxis;
    if(vn!=0){
        dn=(fZ1-pn)/vn;
        if(dn<retval &&dn>0){
            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>0){
            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();
            else if (fabs(z-fZ1)<fThreshold)retval=n->unit();
            else if (fabs(p.mag()-fRmin)<fThreshold)retval=-p.unit();
            else isValid=false;
        }
    }
    if(fabs(retval.mag()-1)>fThreshold &&isValid){
        G4ThreeVector abc=SurfaceNormal(p,isValid);
    }
    return retval;
}