%% Numerik D-MAVT, RW (Nipp), SS06
%
% This is the M-file solution to the eleventh exercise sheet.
% To run, save this file to your account and ensure that the "Current directory"
% in MATLAB is the same as that where the file is saved.
%
%
% Then to run the file type "s11" in the command window

function s10
%Aufgabe 4
disp(['Aufgabe 4']) 
y0=[0.5;0;0;sqrt(3)];   % initial condition
tf=500; tol = 1e-6;
hs=[0.2 0.1]; axiss=[-8 4 -3 9;-3 1 -1.5 2.5];
for i=1:length(hs)
  h=hs(i);
  % integrate using Heun 
  y = y0; orbitH=y(1:2);
  for t=0:h:tf-h
    y = heun(t,y,h,'twobody');
    orbitH = [orbitH y(1:2)];
  end
  % integrate using implicit midpoint rule
  y = y0; orbitM=y(1:2);
  for t=0:h:tf-h
    y = imidfpi(t,y,h,tol,'twobody');
    orbitM = [orbitM y(1:2)];
end
  % plot orbits
  figure;  
  plot(orbitH(1,:),orbitH(2,:),'b:',orbitM(1,:),orbitM(2,:),'g--');
  legend('Heun','imp midpoint');
  xlabel('y_1(t)'); ylabel('y_2(t)');
  title(['h=',num2str(h)]);
  axis(axiss(i,:));
end


function f=twobody(t,y)
d=(y(1)*y(1)+y(2)*y(2))^(1.5);
f=[y(3);y(4);-y(1)/d;-y(2)/d];



function ykp1 = heun(tk,yk,h,f)
% one step of Heun procedure with stepsize h:
% ykp1 = yk + h/2(f(tk,yk)+f(tk+h,yk+h*f(tk,yk)))
fk   = feval(f,tk,yk);
ykp1 = yk + h/2*(fk+feval(f,tk+h,yk+h*fk));


function ykp1 = imidfpi(tk,yk,h,tol,f)

% function ykp1 = imidfpi(tk,yk,h,tol,f)
% 
% one step of implicit midpoint rule with stepsize h:
% ykp1 = yk + h*f(t_{k}+(h/2),(y_k + y_{k+1})/2)
% the arising implicit equation for ykp1(=y_{k+1}) is 
% solved with fixpoint iteration with tolerance tol and 
% starting value y_k+h*f(t_k,y_k) (Euler step)

i=1;
y0 = yk+h*feval(f,tk,yk); % starting value for fixpoint iteration
y  = yk + h*feval(f,tk+h,y0);
while (norm(y-y0)>(1+norm(y))*tol & i<1000)
  y0 = y;
  y  = yk + h*feval(f,tk+(h*0.5),0.5*(yk+y0));
  i  = i+1;
end
if (i==1000) 
  error(['fixpoint iteration did not converge at t=',num2str(tk)]);
else 
  ykp1=y;
end

