clear; clc;

%% Erzeugen einer SPD Matrix
N  = 2;
N1 = N+1;
n  = N1*N1;
d0 = -ones(n-N1,1);
d1 = 4*ones(n,1);
for (i=1:N1)
    d1(i*N1) = 3;
end
d2 = -ones(n-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);
A1 = N^2*D + eye(N1^2);                                       % A1 symmetrisch
svd_A1 = svd(A1);

sort(svd(A1(:)))

for (i=1:n)                                                   % Alle Unterdeterminanten positiv 
    det( A1(1:i, 1:i));                                       % -> Matrix positiv definit  
    if (det (A1(1:i,1:i)) > 0)
        'true'
    else
        'false'
    end
end

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

 %% Matrix auf obere bidiagonale Form bringen (mit Householder Transformationen)
A=A1;
P=eye(n);
Q=eye(n);
 for (k = 1:n)
    x    = A(k:n,k);
    e_s  = eye(n-k+1,1);                            % Einheits(spalten)vektor
    u    = x + s0(x(1))*norm(x,2)*e_s;
    u    = u/norm(u);
    
    A(k:n,k:n) = A(k:n,k:n) - 2*u*(u'*A(k:n,k:n));  % Householdertransformation Spalten
    P(k:n,1:n) = P(k:n,1:n) - 2*u*(u'*P(k:n,1:n));  % Householdertransformation Spalten
    
    if (k <= n-2)
        x   = A(k,k+1:n);
        e_z = eye(1,n-k);                            % Einheits(zeilen)vektor
        v   = x + s0(x(1))*norm(x,2)*e_z;
        v   = v/norm(v);
        A(k:n,k+1:n) = A(k:n,k+1:n) - 2*(A(k:n,k+1:n)*v')*v;    % Householdertransformation Zeilen
        Q(1:n,k+1:n) = Q(1:n,k+1:n) - 2*(Q(1:n,k+1:n)*v')*v;    % Householdertransformation Zeilen
    end
 end

% A          % ist obere Bidiagonalmatrix

norm(A-P*A1*Q)
input('?');
 
 %% Berechne Singulaerwerte von A
 
if (false)
A2 = A'*A;
for k = 1:700 % mit 100 Iterationen bekommt man schon ein relativ genaues Ergebnis
    [Q,R] = qr(A2);
    A2    = R*Q;
end

B = diag(A2);
sw = sqrt(B);       % Vektor der Singulaerwerte (hier: =Eigenwerte)

norm(sort(sw)-sort(svd(A1)))

else

 A = A'*A;
eig(A)

  %% QR Algorithmus zur Berechnung der Eigenwerte

 a = zeros(n,1);
 for (i=1:n)
    a(i) = A(i,i);      % Hauptdiagonale von A
 end

 b = zeros(n-1,1);
 for (i=1:n-1)
    b(i) = A(i,i+1);    % Nebendiagonale(n) von A
 end

 kmax = 10000000;
 x = zeros(n,1);
 y = x;
 g = x;
 c = x;
 s = x;
 d = x;
 e = x;
 for k=1:kmax
    x = a(1);
    y = 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) < 0.00001)
        break
    end
 end

% eig_A = eig(A);       % Vergleiche selbst berechnete Werte mit Eigenwertfunktion von Matlab

sort(a(:))
norm(sort(a(:))-sort(svd_A1(:)))

end
