% Hamiltonian mechanics, lecture 20th April 2015
%
% 2nd example: reduced Kepler problem
% 1st example: harmonic oscillator

clear

l = 1;
m = 1;
gV = @(r) -l./r.^3+m./r.^2;  % Kepler
gV = @(q) q; % harm. oscillator

odefun = @(t,y) [y(2);-gV(y(1))];
tspan = [0, pi];
y0 = [1;0.5];

options1 = odeset('AbsTOl',1e-6);
options2 = odeset('AbsTOl',1e-12);

[t,y1] = ode45(odefun,tspan,y0,options1);
[t,y2] = ode45(odefun,tspan,y0,options2);

close
plot(y1(:,1),y1(:,2),'r',y2(:,1),y2(:,2),'b')


% 1st and 2nd example with Stoermer-Verlet

h = 0.1;
tsteps = floor(tspan(2)/h);

tt(1) = tspan(1);
q(1) = y0(1);
p(1) = y0(2);

for n = 1:tsteps
    tt(n+1) = tt(n) + h;
    
    z0 = [q(n);p(n)];
    z1 = sv(h,gV,z0);
    q(n+1) = z1(1);
    p(n+1) = z1(2);
    
end

hold on
plot(q,p,'g')

% fourth order McLachlan method
s = 5;
w = zeros(s,1);
w(1) = 0.28;
w(2) = 0.62546642846767004501;
w(3) = 1 - 2*(w(1) + w(2));
w(4) = w(2);
w(5) = w(1);

% sixth order Yoshida method
%s = 7;
%w = zeros(s,1);
%w(1) = 0.78451361047755726382;
%w(2) = 0.23557321335935813368;
%w(3) = -1.17767998417887100695;
%w(4) = 1 - 2*(w(1) + w(2) + w(3));
%w(5) = w(3);
%w(6) = w(2);
%w(7) = w(1);

tt(1) = tspan(1);
z(1,1) = y0(1);
z(2,1) = y0(2);

for n = 1:tsteps
    tt(n+1) = tt(n) + h;
    zz = z(:,n);
    
    for j = 1:s
        zz = sv(w(j)*h,gV,zz);
    end
    
    z(:,n+1) = zz;
end

qh = z(1,:);
ph = z(2,:);
hold on
plot(qh,ph,'m')



% harmonic oscillator reference
qr = y0(1)*cos(tt) + y0(2)*sin(tt);
pr = -y0(1)*sin(tt) + y0(2)*cos(tt);
plot(qr,pr,'b')



hold off
semilogy(tt,abs(q-qr),tt,abs(qh-qr)), legend('Stoermer-Verlet','higher order')