% Beispielimplementierung zu Aufgabe 3.5
function []= Blatt3()
clear all; close all; clc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t0=0;
phi0=pi/4;
dotphi0=0;
T=7;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h=0.001;
[t,phi] = PendelExpEuler(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);

h=0.005;
[t,phi] = PendelExpEuler(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);

%%%%%%%%%%%%%%%%%%%%

h=0.001;
[t,phi] = PendelImplEuler(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);

h=0.005;
[t,phi] = PendelImplEuler(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);

%%%%%%%%%%%%%%%%%%%%

h=0.001;
[t,phi] = PendelCrankNicolson(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);

h=0.005;
[t,phi] = PendelCrankNicolson(t0,phi0,dotphi0,h,T);
visualisierePendel(t,phi,T);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Explicit Euler
function [t,phi] = PendelExpEuler(t0,phi0,dotphi0,h,T)

% Systemkonstanten
g=9.81;
l=0.1;
f=g/l;

t=t0:h:T; % Initialisierung des Zeitgitters
l=length(t);

u=zeros(2,l);

u(:,1)=[phi0;dotphi0]; % Startwert

for i=1:l-1 % Expliziter Euler fuer das Pendel
    u(:,i+1)=u(:,i)+h*[u(2,i);-f*sin(u(1,i))];
end

phi=u(1,:);
end
%%%%%%%%%%%%%%%%%%%%%%%
% Implicit Euler
function [t,phi] = PendelImplEuler(t0,phi0,dotphi0,h,T)

% Systemkonstanten
g=9.81;
l=0.1;
const=g/l;

t=t0:h:T; % Initialisierung des Zeitgitters
l=length(t);

u=zeros(2,l);

u(:,1)=[phi0;dotphi0]; % Startwert

for i=1:l-1 % Impliziter Euler fuer das Pendel
    % obtain the value of u(:,i+1) from the equation G(U)=0, where
    % G(U)=U-h*F(U)-u(:,i)
    
    % Newton method
    % U=U_old-inv(G'(U_old))*G(U_old) 
    ul=u(:,i); %initial guess
    max_iter=20;
    tol=10^(-8);
    iter=0;
    err=1000;
    
    while (iter<max_iter)&&(err>tol)
        ul_old=ul;
        F=[ul(2,1);-const*sin(ul(1,1))];
        G=ul-h*F-u(:,i);
        JG=[1 0;0 1] - h*[0 1;-const*cos(ul(1,1)) 0];
        ul=ul-inv(JG)*G;
        err=norm(ul-ul_old);
        iter=iter+1;
    end
    
    u(:,i+1)=ul;
end

phi=u(1,:);
end
%%%%%%%%%%%%%%%%%%%%%%%%%
% Crank-Nicolson
function [t,phi] = PendelCrankNicolson(t0,phi0,dotphi0,h,T)

% Systemkonstanten
g=9.81;
l=0.1;
const=g/l;

t=t0:h:T; % Initialisierung des Zeitgitters
l=length(t);

u=zeros(2,l);

u(:,1)=[phi0;dotphi0]; % Startwert

for i=1:l-1 % Crank-Nicolson fuer das Pendel
    % obtain the value of u(:,i+1) from the equation G(U)=0, where
    % G(U)=U-(h/2)*F(U)-u(:,i)-F(u(:,i))
    
    % Newton method
    % U=U_old-inv(G'(U_old))*G(U_old) 
    ul=u(:,i); %initial guess
    max_iter=20;
    tol=10^(-8);
    iter=0;
    err=1000;
    
    while (iter<max_iter)&&(err>tol)
        ul_old=ul;
        F1=[ul(2,1);-const*sin(ul(1,1))];
        F2=[u(2,i);-const*sin(u(1,i))];
        G=ul-(h/2)*F1-u(:,i)-(h/2)*F2;
        JG=[1 0;0 1] - (h/2)*[0 1;-const*cos(ul(1,1)) 0];
        ul=ul-inv(JG)*G;
        err=norm(ul-ul_old);
        iter=iter+1;
    end
    u(:,i+1)=ul;
end

phi=u(1,:);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot and save
function visualisierePendel(t,phi,T)

p=figure();
%T Endzeit
%(t,phi) diskrete Approximation an den Loesungsgraph
l=0.1; % Laenge des Pendels [m]

t0 = t(1); % Startzeit
h=t(2)-t0; % Schrittweite
phi0 = phi(1); % Startposition
N = length(t);


subplot(1,2,1)
f1 = animatedline();
plot(0,0,'ok','MarkerFaceColor','k');
axis(1.1*l*[-1 1 -1 1])
axis square
hold on
% Zeichne startposition
hl = plot([0 l*sin(phi0)],[0 -l*cos(phi0)],'-k');
drawnow update;
hm = plot(l*sin(phi0),-l*cos(phi0),'o',...
'MarkerSize',10,'MarkerFaceColor','m');
drawnow update;


subplot(1,2,2)
f2 = animatedline();
axis([0 T -4 4])
axis square
grid on
hold on
hy = plot(t0,phi0,'.k');


tic
for k=1:N
    % update f1
    set(hl,'XData',[0 l*sin(phi(k))],'YData',[0 -l*cos(phi(k))]);
    set(hm,'XData',l*sin(phi(k)),'YData',-l*cos(phi(k)));
    % update f2
    addpoints(f2,t(k),phi(k));
    drawnow update
    pause(max(0,h-toc))
    tic
end

% Nach Ablauf der Simulation wird der Graph noch einmal ganz
% gezeichnet, damit man nicht nur den letzten Punkt sieht:
plot(t,phi,'.k');
pause(0.5);

% save image
name=['Blatt3_pic_',num2str(p.Number)];
print(gcf, '-dpdf', name);
end