#include <sstream>
#include <TVector3.h>
#include <RAT/Log.hh>
#include <RAT/QuadraticRoots.hh>

using std::stringstream;
using std::endl;

namespace RAT
{

bool SphereCrossPoint (double SphRad,TVector3& ra,TVector3& rb,
         TVector3& rcross1, TVector3& rcross2, double& f1, double& f2 ) {
    // Given two points, ra and rb, this subroutine returns the points, rcross1
    // and rcross2, at which the line co-inciding with the vector from ra to rb
    // intersects the sphere with radius SphRad.

    // The routine returns false if there is no intersection.
    // If one point is in the same and one is in the opposite direction of the
    // vector from ra to rb, it returns as rcross1 the point in the same
    // direction.
    // If both intersection points in the same direction from ra, rcross1 is set
    // to the point closer to ra.
    // f1 and f2 are the multiples of the distance between ra and rb at which
    // the intersecion points are located.

    // Inputs:
    //    SphRad -- sphere radius
    //    ra -- one input point
    //    rb -- other input point

    double SphRadsq = SphRad*SphRad;
    double rasq = ra.Mag2();

    TVector3 delr = rb - ra;
    double a = delr.Mag2();
    double b = 2.*(ra*delr);    // ra*delr is the dot product
    double c = rasq - SphRadsq;

    bool realFlg = QuadraticRoots(a,b,c,f1,f2);
    if(!realFlg) {
        rcross1 = TVector3(0.,0.,0.);
        rcross2 = TVector3(0.,0.,0.);
        return false;
    }

    if(f2>=0. && f1<0.) {
        double temp = f1;
        f1 = f2;
        f2 = temp;
    }else if(f1<0. && f2<0.) {
        if(f1<f2) {
            double temp = f1;
            f1 = f2;
            f2 = temp;
        }
    }else if(f1>=0. && f2>=0.) {
        if(f1>f2) {
            double temp = f1;
            f1 = f2;
            f2 = temp;
        }
    }

    rcross1 = ra + f1*delr;
    rcross2 = ra + f2*delr;
    return true;
}

}    // namespace RAT