R0 = [0,0,-.5];
V0 = [0,2,0];

T0 = 10^6;

[R,V] = Kepler(R0,V0,T0);

function [R,V] = Kepler(R0,V0,dt)
%--------------------------------------------------------------------------
%Function [R,V] = Kepler(R0,V0,dt)

%This function solves the Kepler problem using the Universal Variable
%Formulation. 

%Given an initial position vector, velocity vector, and change in time, the
%updated position and velocity of the object is given. Expects R0 and V0 as
%row vectors.
%--------------------------------------------------------------------------


gravE = 1;
%Solving for magnitudes of position and velocity vectors &  finding 1/a
%(alpha) and calculating e to determine orbit shape.
r0 = sqrt(R0(1)^2+R0(2)^2+R0(3)^2);
v0 = sqrt(V0(1)^2+V0(2)^2+V0(3)^2);
h0 = dot(R0,V0);
%alpha is 1/a. any place where 1/a appears is replaced with alpha. 
alpha = ((2*gravE/r0)-(v0^2))/gravE;
e = sqrt(1- (h0^2/gravE*alpha));
disp(e);

%Newton Iteration to Find x
%1. Guess value of x%

x(1) = sqrt(gravE)*(dt*alpha);
error =2;

%2. Completing Newton Iteration to find value of x
while error > 1E-7
    z = ((x)^2*alpha);
    if z == 0
        syms k
        C = symsum(((-z)^k)/factorial((2*k)+2),k,0,50);
        S = symsum(((-z)^k)/factorial((2*k)+3),k,0,50);
    elseif z < 0
        C = (1-cosh(sqrt(-z)));
        S = (sinh(sqrt(-z))-sqrt(-z))/sqrt(-z^3);
    else
        C = (1-cos(sqrt(z)))/z;%add case which computes C & S as series when z = 0
        S = (sqrt(z)-sin(sqrt(z)))/sqrt(z^3); 
    end
    t =(1/sqrt(gravE))*((1/sqrt(gravE))*dot(R0,V0)*x^2*C+(((1-r0*alpha)*(x^3)*S)+(r0*x)));   
    dt_dx = (1/sqrt(gravE))*((x^2)*C+(1/sqrt(gravE))*dot(R0,V0)*(1-z*S)+(r0*(1-z*C)));
    x =x+(dt-t)/dt_dx;
    error = abs((dt-t)/dt);
    w = [x,z,t];
    disp(w);
  
end

%Solving for f, g, R, and r
f = 1-(x^2/r0*C);
g = t - x^3*S/sqrt(gravE);
R = (f*R0)+(g*V0);
r = sqrt(R(1)^2+R(2)^2+R(3)^2);

%Solving for fdot, gdot, & v
gdot = 1-(x^2*C/r);
fdot = (sqrt(gravE)/r0/r*x)*(z*S-1);
V = (fdot*R0)+(gdot*V0);
end