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

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

namespace RAT
{

TVector3 SphereCrossPoint(double SphRad,TVector3& ra,TVector3& rb) {
    // Given two points, ra and rb, with ra inside the sphere of radius SphRad,
    // this subroutine finds the point at which the vector from ra to rb
    // intersects the sphere.

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

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

    if(rasq>SphRadsq) {
        stringstream msg;
        msg << "\nSphereCrossPoint: first point outside sphere; "
            << "SphRad, point r,x,y,z = " << SphRad <<" "<< ra.Mag()
            <<" "<<  ra.X() <<" "<<  ra.Y() <<" "<<  ra.Z() ;
        RAT::Log::Die( msg.str() );
    }

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

    double f,f1,f2;
    bool realFlg = QuadraticRoots(a,b,c,f1,f2);
    if(!realFlg) {
        stringstream msg;
        msg << "SphereCrossPoint: unexpected imaginary solution; "
            << "a, b, c = "<< a <<"\t"<< b <<"\t"<< c ;
        RAT::Log::Die( msg.str() );
    }

    if(f1>=0. && f2<=0.) {
        f = f1;
    }else if (f2>=0. && f1<0.) {
        f = f2;
    }else{
        stringstream msg;
        msg <<"SphereCrossPoint: inconsistent result;  f1, f2 = "
            << f1 <<"\t"<< f2<<endl;
        RAT::Log::Die( msg.str() );
    }

    TVector3 r = ra + f*delr;
    return r;
}

}    // namespace RAT