% Variable z = [x,y,v,w]^T.
numstep=10000; % 10000
h=0.005;
z=zeros(4,numstep);
e=zeros(1,numstep);
z(:,1)=[1 1 0 1]';
e(1)=energyval(z(:,1)); % = E0

% AB1
for i=2:numstep
    z(:,i)=z(:,i-1)+h*funval(z(:,i-1));
    e(i)=energyval(z(:,i));
end
plot(z(1,:),z(2,:));
title('AB1 trajectory');
figure
plot(1:numstep,e-e(1));
title('AB1: Energy difference');

% AB2
% Compute z1 with Runge-Kutta
fn_1 = funval(z(:,1));
k1=h*funval(z(:,1));
k2=h*funval(z(:,1)+k1);
z(:,2)=z(:,1)+(k1+k2)/2;
fn = funval(z(:,2));

for i=2:numstep
    z(:,i)=z(:,i-1)+h/2*(3*fn-fn_1);
    fn_1=fn;
    fn=funval(z(:,i));
    e(i)=energyval(z(:,i));
end
figure
plot(z(1,:),z(2,:));
title('AB2 trajectory');
figure
plot(linspace(0,50,numstep),e-e(1));
title('AB2: Energy difference');
