clc; clear;
n = 30;
A = rand(n,n);
A = A'*A;

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

merke = A;
P = eye(n); Q = eye(n);
for k = 1:(n-1)
    u     = A(k:end, k);
    u_inf = norm(u, inf);
    u     = u/u_inf;
    rho   = si(u(1))*norm(u, 2);
    u(1)  = u(1) + rho;
    theta = rho*u(1);
    sigma = rho*u_inf;
    u     = [zeros(k-1, 1); u];
    PP    = eye(n) - u*u'/theta;
    
    A = PP*A;
    P = PP*P;
    
    if (k ~= n-1)
        u     = A(k, k+1:end)';
        u_inf = norm(u, inf);
        u     = u/u_inf;
        rho   = si(u(1))*norm(u, 2);
        u(1)  = u(1) + rho;
        theta = rho*u(1);
        sigma = rho*u_inf;
        u     = [zeros(k, 1); u];
        QQ    = eye(n) - u*u'/theta;
        
        A = A*QQ;
        Q = Q*QQ;
    end
end
A = merke;
clear merke;

B = P*A*Q;
if n<=10;
    disp('P*A*Q');
    disp(num2str(B,'%11.4f'));
end


%% Bidiagonal -> Singulaerwerte


x = zeros(n,1); 
y = zeros(n-1,1);
c = zeros(n-1,1);
s = zeros(n-1,1);
g = zeros(n-1,1);
d = ones(n-1,1);
e = zeros(n-2,1);
it = 0;
it_max = 2000;
tol = 1e-15;

while ((it < it_max) && (norm(d,1) > tol*norm(g,1)))
    BB = B'*B;
    a    = diag(BB);
    b    = diag(BB,-1);
    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);
    Q = eye(n);
    for j = 1:n-1
        q1 = Q(:, j)*c(j) + Q(:, j+1)*s(j);
        q2 = Q(:, j+1)*c(j) - Q(:,j)*s(j);
        Q(:, j)   = q1;
        Q(:, j+1) = q2;
    end
    BQ = B*Q;
    a = diag(BQ);
    b = diag(BQ, -1);
    x(1) = a(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)   = s(j)*a(j+1);
        x(j+1) = c(j)*a(j+1);
	end
	g(n) = x(n);
    for i = 1:(n-1)
        B(i, i)   = g(i);
        B(i, i+1) = d(i);
    end
    B(n,n) = g(n);
    
    it = it + 1;
end
g = abs(g);

sv = svd(A);
disp('PROBE:');
disp(['Anzahl der Iterationen: ', num2str(it)]);
disp(['norm(sort(sv)-sort(g), 1) = ', num2str(norm(sort(sv)-sort(g), 1))]);

if n<=10
    disp('Eigenwerte: ');
    disp(num2str(sort(g)));
    disp('eig(A): ');
    disp(num2str(sort(sv)));
end

