%function Adanc_SK

%Pseudo-Code zur Transformation auf Bidiagonale Form
m=3;
n=3;
P=eye(m);
Q=eye(n);
A=rand(m,n);
B=zeros(n,n);

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

c = zeros(1,n-1);
d = zeros(m,1);
C = zeros(m,n-1);
for i=1:m
    d(i)=A(i,1);
    for j=1:n-1
        C(i,j)=A(i,j+1);
    end
end

for k=2:n+1
%
    u = zeros(m-k+2,1);
    for i=1:m-k+2
        u(i) = d(i);
    end
    uinf = norm(u,inf);
    u = u/uinf;
    sigma = s(u(1))*norm(u,2);
    u(1) = u(1) + sigma;
    theta = sigma*u(1); 
    sigma = sigma*uinf;
%
    B(k-1,k-1)=-sigma;
    if k>2
        B(k-2,k-1)=a;
    end
    for l=1:n-k+1
        sm=0;
        for i=1:m-k+2
            sm=sm+C(i,l)*u(i);
        end
        for i=1:m-k+2
            C(i,l)=C(i,l)-sm*u(i)/theta;
        end
    end
    for l=1:m
        sm=0;
        for i=1:m-k+2
            sm=sm+P(k-2+i,l)*u(i);
        end
        for i=1:m-k+2
            P(k-2+i,l)=P(k-2+i,l)-sm*u(i)/theta;
        end
    end
    if k<n
        for j=1:n-k+1
            c(j)=C(1,j);
        end
%
        u = zeros(1,n-k+1);
        for i=1:n-k+1
            u(i) = c(i);
        end
        uinf  = norm(u,inf);
        u     = u/uinf;
        tau   = s(u(1))*norm(u,2);
        u(1)  = u(1) + tau;
        theta = tau*u(1); 
        tau   = tau*uinf;
%
        a= -tau;
        for l=2:m-k+2
            sm=0;
            for i=1:n-k+1
                sm=sm+C(l,i)*u(i);
            end
            for i=1:n-k+1
                C(l,i)=C(l,i)-sm*u(i)/theta;
            end
        end
        for l=1:n
            sm=0;
            for i=1:n-k+1
                sm=sm+Q(l,k-1+i)*u(i);
            end
            for i=1:n-k+1
                Q(l,k-1+i)=Q(l,k-1+i)-sm*u(i)/theta;
            end
        end
    end

    if k==n
        a=C(1,1);
    end
    if k<n+1
        for i=1:m-k+1
            d(i)=C(i+1,1);
            for j=1:n-k
                C(i,j)=C(i+1,j+1);
            end
        end
    end
end

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

kmax=1000;
tol=1.0e-15;

x=zeros(n,1); g=x; y=zeros(n-1,1); d=y; c=y; s=y; e=zeros(n-2,1);

for k=1:kmax
    B0=B'*B;
    for i=1:n
        a(i)=B0(i,i);
    end
    for i=1:n-1
        b(i)=B0(i+1,i);
    end
    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);
    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
    BQ=B*Q;
    for i=1:n
        a(i)=BQ(i,i);
    end
    for i=1:n-1
        b(i)=BQ(i+1,i);
    end
    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
end
g = abs(g);    
norm(sort(g)-sort(svd(A)))

