%% Numerik Blatt 4 Bsp6
%Loese Ax = b mit der Methode der konjugierten Gradienten
clear;clc;
tic

%Anfangsbedingungen
N = 30;
N1 = N+1;
N1N1 = N1*N1;
N4 = round(N/4);
V0 = [zeros(N4,1);ones(N1-2*N4,1);zeros(N4,1)];
V0 = kron(V0,V0');
V0 = reshape(V0,N1N1,1);

h = 1/N;
r = 1;
d = 1;           % Fuer d<1 schlechte Approximation, Flaeche geht nicht ganz nach 0
                 % CG Verfahren funktioniert nicht fuer d = 0;
k = 0;           % Fuer k~=0 Konvergenz im CG Verfahren nicht garantiert.
                 % (Hier funktioniert es erst bei k=10 nicht mehr)

%Diagonalen fuer Matrix D erzeugen
d0 = -ones(N1N1-N1,1);
d1 = 4*ones(N1N1,1);
for i=1:N1
    d1(i*N1)=3;
end
d2 = -ones(N1N1-1,1);
for j=1:N1-1
    d2(j*N1)=0;
end

D = diag(d1)+diag(d2,1)+diag(d2,-1)+diag(d0,N1)+diag(d0,-N1);
F = eye(N1^2)-diag(ones(N1^2-1,1),-1);

B = d/h^2*D + k/h*F + r*eye(N1^2);

M = 20;
tau = 1/M;                      %tau sehr klein => schlechtere Approximation
A = eye(N1N1) + tau*B;
D = diag(diag(A));
imax = 500;                 % Maximale Iterationen

if d == 0 | k~=0             % Fuer d=0 oder k~=0 loese mit backslash!(funktioniert immer)
    
    while i<imax
          V0 = A\V0;
          i = i+1;
          surf(reshape(V0,N1,N1));
          axis([1 N1 1 N1 0 1]);
          view([1 1 1]); drawnow; pause(0.1);
    end
else

%% CG Verfahren
    for n = 0:M
        r = V0-A*V0;
        eps = 1.0e-4;              % Toleranzgrenze
        tau = zeros(imax,1);
        ro = norm(r)^2;
        i = 2;

        while i<imax && sqrt(ro)>eps*norm(V0)
            z = D\r;
            tau(i-1) = z'*r;

            if i==2
                beta = 0;
                p = z;
            else
                beta = tau(i-1)/tau(i-2);
                p = z + beta*p;
            end

            w = A*p;
            alpha = tau(i-1)/(p'*w);
            V0 = V0 + alpha*p;
            r = r - alpha*w;
            ro = r'*r;
            i = i+1;
        end

     surf(reshape(V0,N1,N1));
     axis([1 N1 1 N1 0 1]);
     view([1 1 1]); drawnow; pause(0.1);
    end
xlabel('x-Achse')
ylabel('y-Achse')
end
toc
