function [r_vec,v_vec,X] = nma_moveProbe(r0_vec,v0_vec,deltaTime,mu)
%Moves probe in an orbit for deltaTime 

%function [r_vec,v_vec] = nma_moveProbe(r0_vec,v0_vec,tof,mu)
%
% Moves a probe in an orbit for deltaTime from a starting
% position and velosity vectors, and returns the new position
% and veclosity vectors.
%
% INPUT:
%   r0_vec : Vector. The initial position vector. An array of 3 numbers.
%   vo_vec : Vector. The initial velosity vector. An array of 3 numbers.
%   mu     : Scalar. The gravitional parameter. 
%
% OUTPUT:
%   r_vec : Vector. The new position vector. An array of 3 numbers.
%   v_vec : Vector. The new velosity vector. An array of 3 numbers.
%   X     : scalar. The universal variable X for this TOF.

X = nma_getUniversalVariable(r0_vec,v0_vec,deltaTime,mu);

%
% Now that we have X for this probe, we find future position 
% velosity
%
o=nma_getOrbitParams(r0_vec,v0_vec,mu);

alpha=1/o.a;
y=alpha*X^2;
r0= norm(r0_vec);

f=  1 - (X^2/r0)*C(y);
g=  deltaTime- (X^3/sqrt(mu)) * S(y);

r_vec= f*r0_vec + g*v0_vec;

r= norm(r_vec);

ff= X*sqrt(mu)/(r*r0) * (alpha*X^2 * S(y) - 1);
gg= 1 - (X^2/r)*C(y);

v_vec= ff*r0_vec + gg*v0_vec;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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