function Glowatschnig_SK

%Transformation auf Bidiagonalform

n = 3;
m = 3;
%I = eye(n,m);
A = rand(m,n); % Zufallsmatrix
%A = A*A'+I; % symmetrisch + SPD

d = A(:,1);
C = A(1:m,(1:n-1)+1);
P = eye(m);
Q = eye(n);
B = zeros(n,n);
    for k = 2:n+1
        [u,theta,sigma] = householder(d);
        B(k-1,k-1) = -sigma;
        if (k>2)
            B(k-2,k-1) = a;
        end
        for l = 1:n-k+1
           sm = dot(C(1:m-k+2,l),u(1:m-k+2)) ;        
           C(1:m-k+2,l) = C(1:m-k+2,l) - sm*u(1:m-k+2)/theta;
        end
        for l = 1:m
            sm = dot(P(k-2+(1:m-k+2),l),u(1:m-k+2));
            P(k-2+(1:m-k+2),l) = P(k-2+(1:m-k+2),l) - sm*u(1:m-k+2)/theta;
        end
        if (k<n)
            c = C(1,1:n-k+1);
            [u,theta,tau] = householder(c);
             a = -tau;
             for l =2:m-k+2
                 sm = dot(C(l,(1:n-k+1)), u(1:n-k+1));
                 C(l,(1:n-k+1)) = C(l,(1:n-k+1)) - sm*u(1:n-k+1)/theta;
             end
             for l=1:n
                 sm = dot(Q(l,(k-1+(1:n-k+1))),u(1:n-k+1));
                 Q(l,(k-1+(1:n-k+1))) = Q(l,(k-1+(1:n-k+1))) - sm*u(1:n-k+1)/theta;
             end
        end
        if (k == n)
            a =C(1,1);
        end
        if ( k < (n+1))
           d = C((1:m-k+1)+1,1);
           C = C((1:m-k+1)+1,(1:n-k)+1);
        end   
    end

PAQ = P*A*Q;
PAQ = PAQ(1:n,1:n);
norm(B-PAQ)
input('?');

kmax = 1000;
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);
q2 = zeros(n,1);

while (k < kmax)
    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)=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);
    R = diag(g) + diag(d,1) + diag(e,2);
    Q = eye(n);
    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);
        end
        for i = 1:n
            Q(i,j) = q1(i);
            Q(i,j+1) = q2(i);
        end
    end

%norm(B'*B-Q*R)
%input('?');

    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);
    
    B = zeros(n,n);
    for i = 1:n
        B(i,i) = g(i);
    end
    for i=1:n-1
        B(i,i+1)=d(i);
    end  

%norm(B'*B-R*Q)
%input('?');

    if (norm(d) < tol*norm(g))
        break;
    end
    k = k+1;
end

norm(sort(g)-sort(svd(A)))

end

%%-------------------------------------------------------

function [u,theta,sigma] = householder(x)
    s     = @(t) double(t>=0) - double(t<0);
    xinf  = norm(x,inf);
    v     = x/xinf;
    rho   = s(v(1))*norm(v,2);
    u     = v;
    u(1)  = u(1) + rho;
    theta = rho*u(1);
    sigma = rho*xinf;
end
