clear
clc

%----------- Einf. i. d. Numerik Übungsblatt 6 -----------
% Marlene Lepuschitz

%--------------a)----------------
n = 7;
A=rand(n);
%Erzeugen von SPD Matrix
A = A*A'+eye(n);
Asav = A;
Psav = eye(n);

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

%zuerst auf Tridiagonalform bringen um dann leichter
%auf Bidiagonalform umzuformen
alpha = -s0(A(2,1))*sqrt(sum(A(2:n,1).^2));
r     = sqrt((1/2)*(alpha^2-A(2,1)*alpha));
v1    = zeros(n);
v1(2) = (A(2,1)-alpha)/(2*r);
for k=3:n
    v1(k)=(A(k,1))/(2*r); 
end

P1 = eye(n)-2*v1*v1';
A  = P1*A*P1;

Psav = P1*Psav;

for k=2:n-2
    alpha = -s0(A(k+1,k))*sqrt(sum(A(k+1:n,k).^2));
    r     = sqrt((1/2)*(alpha^2-A(k+1,k)*alpha));
    v     = zeros(n);
    K     = k+1;
    v(K)  =(A(k+1,k)-alpha)/(2*r);
    for j=k+2:n
        v(j) = A(j,k)/(2*r);
    end
    P = eye(n)-2*v*v';
Psav = P*Psav;
    A = P*A*P;
end
A_trid=A

norm(A - Psav*Asav*Psav')
input('?');

% Bidiagonalisierung

Q = eye(n);

for k=1:1
    r          = sqrt(A(k,k)^2+A(k+1,k)^2);
    c          = A(k,k)/(r + (r == 0))*(r ~= 0);
    s          = A(k+1,k)/(r + (r == 0))*(r ~= 0);
    G          = eye(n);
    G(k,k)     = c;
    G(k,k+1)   = s;
    G(k+1,k)   =-s;
    G(k+1,k+1) = c;
%    A_bidiag   = horzcat(G*A(:,1),A(:,2:n));
    A = G*A;
    Q = G*Q;
end

for k=2:(n-1)
    r          = sqrt(A(k,k)^2+A(k+1,k)^2);
    c          = A(k,k)/(r + (r == 0))*(r ~= 0);
    s          = A(k+1,k)/(r + (r == 0))*(r ~= 0);
    G          = eye(n);
    G(k,k)     = c;
    G(k,k+1)   = s;
    G(k+1,k)   = -s;
    G(k+1,k+1) = c;
%    A_bidiag=horzcat(A_bidiag(:,1:k-1),G*A(:,k),A(:,k+1:n));
    A = G*A;
    Q = G*Q;    
end
A_bidiag = A;

norm(A_trid-Q'*A)
input('?');

%-------------b)-----------------
%Singulaerwert Zerlegung
B    = A_bidiag;
kmax = 20;
n    = 7;
tol  = 1e-10;
k    = 0;
x = zeros(n,1);
y = zeros(n-1,1);
g = zeros(n,1);
c = zeros(n-1,1);
s = zeros(n-1,1);
d = zeros(n-1,1);
e = zeros(n-2,1);
q1 = zeros(n-1,1);
q2 = zeros(n-2,1);
BB = B'*B;
 
while (k < kmax)
for k=1:20
%    a = BB(1:n-1,1:n);
%    b = BB((1:n-1)+1,1:n-1); 
    a = diag(BB);
    b = diag(BB,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) = c(j)*a(j+1)-s(j)*y(j);
        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(size(B));
    for j = 1:n-1
        for i = 1:n
        q1(i)    = Q(i,j)*c(j)+Q(i,j+1)*s(j);
        q2(i)    = Q(i,j+1)*c(j)-Q(i,j)*s(j);
        Q(i,j)   = q1(i);
        Q(i,j+1) = q2(i);
        end
    end
end

    BQ    = B*Q;
    a     = BQ(1:n,1:n);
    b     = BQ((2:n-1)-1,2:n-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
    B(i,i) = g(i);
    end
    for i=1:n-1
    B(i,i+1)=d(i);
    end  
       if norm(d) < tol * norm(g)
       break
       end
    k = k+1;
end

Singulaerwerte = abs(g)
Test=svd(Asav)
