clear
mu = 1;
e=0.9;
aa=1;
x0=aa*(1-e);
v0=sqrt(mu*(1+e)/(1-e)/aa);

figure(1)

tspan=[0 100];
z0=[x0;0;0;v0];
[t1,z] = ode45('twobodyf',tspan, z0);
plot(z(:,1),z(:,2),'r');
axis equal

figure(2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Euler & Cromer Methods
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initial Condition
t(1)=0;
x(1)=x0;
u(1)=0;
y(1)=0;
v(1)=v0;

icase=3;

% Parameters for scheme
a=0;
b=100;

if icase == 1
    N=4000000; % Euler
elseif icase == 2
    N=200000;  % Semi-implicit Euler
else
    N=50000;  % Velocity Verlet
end
h=(b-a)/N;

if icase == 1
    for i=2:N+1 
        alpha=mu/(x(i-1).^2 + y(i-1).^2).^(3/2);
        u(i)=u(i-1)-h*alpha*x(i-1);
        v(i)=v(i-1)-h*alpha*y(i-1);
        x(i)=x(i-1)+h*u(i-1);
        y(i)=y(i-1)+h*v(i-1);
        t(i)=t(i-1)+h;
    end
elseif icase == 2
    for i=2:N+1
        alpha=mu/(x(i-1).^2 + y(i-1).^2).^(3/2);
        u(i)=u(i-1)-h*alpha*x(i-1);
        v(i)=v(i-1)-h*alpha*y(i-1);
        x(i)=x(i-1)+h*u(i);
        y(i)=y(i-1)+h*v(i);
        t(i)=t(i-1)+h;
    end
else
     for i=2:N+1
        alpha=mu/(x(i-1).^2 + y(i-1).^2).^(3/2);
        x(i)=x(i-1)+h*u(i-1)-h^2/2*alpha*x(i-1);
        y(i)=y(i-1)+h*v(i-1)-h^2/2*alpha*y(i-1);
        u(i)=u(i-1)-h/2*alpha*x(i-1);
        v(i)=v(i-1)-h/2*alpha*y(i-1);
        alpha=mu/(x(i).^2 + y(i).^2).^(3/2);
        u(i)=u(i)-h/2*alpha*x(i);
        v(i)=v(i)-h/2*alpha*y(i);
        t(i)=t(i-1)+h;
    end
end

plot(x,y)
axis equal
figure(1)
hold on
plot(x,y)
hold off

figure(3)
% Conservation
L=x.*v-y.*u;
plot(t,L)
hold on
L2=z(:,1).*z(:,4)-z(:,2).*z(:,3);
plot(t1,L2,'r')
plot(t1,sqrt(mu*aa*(1-e^2)),'k')
xlabel('Time')
ylabel('Angular Momentum')
hold off

r2=x.^2+y.^2;
v2=u.^2+v.^2;
r=sqrt(r2);
E=1/2*v2-mu./r;
figure(4)
plot(t,E)
xlabel('Time')
ylabel('Energy')
hold on

r2b=z(:,1).^2+z(:,2).^2;
v2b=z(:,3).^2+z(:,4).^2;
rb=sqrt(r2b);
Eb=1/2*v2b-mu./rb;
plot(t1,Eb,'r')
plot(t1,-mu/2/aa,'k')
hold off

% Plot parametric Equations
phi=0:.01:2*pi;
r=aa*(1-e^2)./(1+e*cos(phi));
xx=r.*cos(phi);
yy=r.*sin(phi);
figure(2)
hold on
plot(xx,yy,'k','LineWidth',2)
hold off