function X=nma_getUniversalVariable(r0_vec,v0_vec,tof,mu)
%compute the Universal Variable X for an orbit

%function X=nma_getUniversalVariable(r0_vec,v0_vec,tof,mu)
%
% compute the Universal Variable X for an orbit.
%
% INPUT:
%   r0_vec : Vector. The position vector at epoch. An array of 3 numbers.
%   vo_vec : Vector. The velosity vector at epoch. An array of 3 numbers.
%   tof    : scalar. The time of flight in the appropiate time units.
%   mu     : Scalar. The gravitional parameter. 
%
% OUTPUT:
%   X:  scalar. The universal variable.
%
% Example usage:
%
%   mu=1;
%   v0_vec = [1 2 -4.4];   % v0_vec = I + 2J  + 4.4K
%   r0_vec = [2 3 4];      % r0_vec = 2I + 3J + 4 K
%   tof=10;
%   X = nma_getUniversalVariable(r0_vec,v0_vec,tof,mu);
%
% Algorithm:
%   using the epoch position and velosity vectors, find orbit parameters.
%   Next, solve for X using equation 4.4-14, page 197, Bates, Mueller,
%   White book. This function will use laugerre method to solve for X.
%
%
% Author: Nasser Abbasi
% Version: 1.0

%change history:
%
% name   date    changes
%-----  ------   ---------
% nma   4/30/03  Initial v1.0

TRUE=1;
FALSE=0;

TOL=0.001; % perecntage tolerance to stop searching for X for laugerre method.

o = nma_getOrbitParams(r0_vec,v0_vec,mu);

alpha  =  1/o.a;
sigma0 =  dot(r0_vec,v0_vec)/sqrt(mu);
r0     =  norm(r0_vec);

X0= guessAnInitialX(tof,o.p,norm(o.e_vec),mu);

%
% start the laugerre loop
%
keepLookingForX = TRUE;

X=X0;

n=5;  % known to be best :)

while(keepLookingForX)
   f=F(X,sigma0,r0,alpha,tof,mu);
   fder=Fder(X,sigma0,r0,alpha);
   f2der=F2der(X,sigma0,r0,alpha);

   if(fder<0)
     sign=-1;
   else
     sign=1;
   end
   
   term = sqrt( abs( ( (n-1)*fder )^2 - n*(n-1)*f*f2der) );                         
   term = (1*sign)* term;
   
   newX = X  -  ( n*f / (fder + term) );
 
   if(  abs( (newX - X)/newX ) * 100  < TOL )
       keepLookingForX = FALSE;
   end

   X = newX;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function v=S(y)

if(y>0)
    v= ( sqrt(y) - sin(sqrt(y)) ) / sqrt(y^3) ;
elseif(y<0)  % hyparabola
    v= ( sinh(sqrt(-y)) - sqrt(-y)  ) / sqrt(-y^3);
else
    v=1/6;  % parabola
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function v=C(y)

if(y>0)
    v= (1-cos(sqrt(y)))/y;
elseif(y<0)  % hyparabola
    v= (1-cosh(sqrt(-y)))/y;
else
    v=1/2;  % parabola
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = F(X,sigma0,r0,alpha,tof,mu)

arg=alpha*X^2;

f= sigma0 * X^2 * C(arg)   + (1-r0*alpha)*X^3 * S(arg)  +   r0*X - sqrt(mu)*tof;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fder = Fder(X,sigma0,r0,alpha)

arg=alpha*X^2;

fder= sigma0*X*( 1 - arg * S(arg) ) + (1 - r0*alpha) * X^2 * C(arg) + r0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f2der = F2der(X,sigma0,r0,alpha)

arg=alpha*X^2;

f2der= (1-r0*alpha)*X*( 1 - alpha*X^2 *S(arg) ) + sigma0*(1- arg*C(arg));
 

%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%
function X0=guessAnInitialX(tof,p,e,mu)

r_p = p/(1+e);
X_upperLimit = sqrt(mu)*(tof)/r_p;

if(e>=1) % parabola and hyparabola
    X_lowerLimit=0;
else
    r_A = p/(1-e);    
    X_lowerLimit = sqrt(mu)*(tof)/r_A;    
end

X0 = ( (X_lowerLimit)+(X_upperLimit) )/ 2;