%% Beispiel 6 Blatt 5
clc; clear; 

N = 1;
N1 = N+1;
n = N1*N1;
D0 = -ones(n,1); 
A = spdiags(D0,-N1,n,n) + spdiags(D0,N1,n,n) + spdiags(D0,1,n,n) ...
    + spdiags(D0,-1,n,n) + speye(n)*4; D(n,n) = 3;
n = size(A,1);

%% Bidiagonalisierung der Matrix A

s0 = @(t) double(t>=0) - double(t<0);

[m,n] = size(A);
B = A;
P = eye(m);
Q = eye(n);
for k = 1:n
    x = B(k:n,k);
    e1 = eye(n-k+1,1);
    uk = x + s0(x(1))*norm(x,2)*e1;
    uk = uk/norm(uk,2);
    B(k:n,k:n) = B(k:n,k:n) - 2*uk*(uk'*B(k:n,k:n));
    P(k:n,1:n) = P(k:n,1:n) - 2*uk*(uk'*P(k:n,1:n));
    if k <= n-2
        x = B(k,k+1:n);
        e2 = eye(1,n-k);
        vk = x + s0(x(1))*norm(x,2)*e2;
        vk = vk/norm(vk,2);
        B(k:n,k+1:n) = B(k:n,k+1:n) - 2*(B(k:n,k+1:n)*vk')*vk;
        Q(1:n,k+1:n) = Q(1:n,k+1:n) - 2*(Q(1:n,k+1:n)*vk')*vk;
    end
end
disp(strcat('||P*A*Q-B||=',num2str(norm(P*A*Q-B))))

%% Singulärwertzerlegung der Matrix A

H = B'*B;

a = diag(H);
b = zeros(1,n);
x = b; y = b; g = b; c = b; d = b; s = b; e = b;
for i = 1:n-1
    b(i) = H(i,i+1);
end
kmax = 1000;
eps = 1*e-10;
for k = 1:kmax;
    x(1) = a(1); y(1) = b(1);
    for j = 1:n-1
        g(j) = sqrt(x(j)^2 + b(j)^2);
        c(j) = x(j)/g(j); s(j) = b(j)/g(j);
        d(j) = c(j)*y(j) + s(j)*a(j+1);
        x(j+1) = -s(j) * y(j) + c(j)*a(j+1);
        if j ~= n-1
            e(j) = s(j)*b(j+1); y(j+1) = c(j)*b(j+1);
        end
    end
    g(n) = x(n);
    a(1) = s(1)*d(1)+c(1)*g(1); b(1)= s(1)*g(2);
    for j = 2:n-1
        a(j) = s(j)*d(j) + c(j)*c(j-1)*g(j);
        b(j) = s(j) * g(j+1);
    end
    a(n) = c(n-1)*g(n);
    if(norm(b)< eps) 
        break
    end
end

S = sqrt(a);
disp(strcat('||S-svd(A)||=',num2str(norm(sort(S(:))-sort(eig(A))))))
