SVD "High Accurancy" Demmel - Kahan Algorithm

Hi guys! I'm trying to write the SVD algorithm from Demmel - Kahan ("Computing Accurate Singular Value Decomposition") in MATLAB.

I wrote almost all the code but i don't understand some steps of this algorithm.

The step where the algorithm says "Handle specially, Compute SVD of the 2x2 submatrix from iLower to iUpper+1"...do i have to apply the zero-shift QR diagonalization to the 2x2 submatrix?

Then when it says that I have to apply the standard shifted algorithm it doesn't explain how to do it just using the diagonal and upper diagonal vectors from the bidiagonal matrix.

However, I don't think that the problem is only in these steps because sometimes my code works and the result is just like that from the svd() built-in function in matlab.

I'll link the article with the algorithm from Demmel - Kahan and the code that I wrote here, maybe someone can help me.

Thanks!

    %%%%%%%High Accuracy Bidiagonal SVD %%%%%%%
dir=0;
eps = 1e-010; 
tol = 100*eps; 
maxit = 3*(n^2); 
fudge=min(m,n); 
s=diag(B);
e=diag(B,1);
lambda=abs(s);
for j=n-1:-1:1
    lambdas(j)=abs(s(j))*(lambda(j+1)/(lambda(j+1)+abs(e(j))));
end
mu(1)=abs(s(1));
for j = 1:n-1
    mu(j+1) = abs(s(j+1))*(mu(j)/(mu(j)+abs(e(j))));
end
lambdaMin=min(lambdas);
muMin=min(mu);
sigmaLower = min(lambdaMin,muMin);
s1=max(abs(s));
e1=max(abs(e));
sigmaUpper = max(s1,e1);
thresh = max((tol*sigmaLower),(maxit*realmin));
for iter=1:maxit
    for i=1:n-1
    if abs(e(i))<=thresh
      iUpper=i;
    else iUpper=n;
    end
  end
  if iUpper==1  
    break;
  end
  
  for i=n-1:-1:1
    if abs(e(i))<=thresh
      iPrime=i;
    else iPrime=0;
    end
  end
  iLower=iPrime+1;

        tmp=B(iLower:iUpper,iLower:iUpper);
  if iUpper==iLower+1
                [s,e]=ZeroShiftQR(s,e,iLower,iUpper,1);
                e(iLower)=0;
    B(iLower:iUpper,iLower:iUpper)=diag(s)+diag(e,1);
    continue;
  end
        if B(iLower:iUpper,iLower:iUpper)~=tmp
    s=diag(B);
    e=diag(B,1);
    if abs(s(iLower))<=abs(s(iUpper))
      dir=1;
    else dir=2;
    end
  end
    % Convergence criteria
    if dir==1 % Convergence criteria 1b,1a,2a
        if abs(e(iUpper-1)/lambdas(iUpper))<=tol % 1b to e(iUpper-1)
            e(iUpper-1)=0;
        end
        for j=1:n-1 % Criteria 1a
            if abs(e(j)/mu(j))<=tol
                e(j)=0;
            end
        end
        % Criteria 2a to e(iUpper-1)
        tmp=mu(1)/sqrt(n-1);
        for i=2:n-1
            if mu(i)/sqrt(n-1)<tmp
                tmp=mu(i)/sqrt(n-1);
            end
        end
        if e(iUpper-1)*e(iUpper-1)<=0.5*tol*(tmp*tmp-abs(s(iUpper))*abs(s(iUpper)))
            e(iUpper-1)=0;
        end
    else % Convergence criteria 1a,1b,2b
        if abs(e(iLower)/mu(iLower))<=tol % Criteria 1a to e(iLower)
            e(iLower)=0;
        end
        for j=n-1:-1:1 % Criteria 1b
            if abs(e(j)/lambdas(j))<=tol
                e(j)=0;
            end
        end
        % Criteria 2b 
        tmp=lambdas(2)/sqrt(n-1);
        for i=3:n-1
            if lambdas(i)/sqrt(n-1)<tmp
               tmp=lambdas(i)/sqrt(n-1);
            end
        end
        if e(iLower)*e(iLower)<=0.5*tol*(tmp*tmp-abs(s(iLower))*abs(s(iLower)))
            e(iLower)=0;
        end
    end
    % Compute the shift
  if (fudge*tol*sigmaLower/sigmaUpper)<=eps
        shift=0;
    else if dir==1
            w=s(iUpper);
            C=diag(s)+diag(e,1);
            C=C*C';
            C=C(end-1:end,end-1:end);
            [lambda_1,lambda_2] = eig2(C); % Calculate the eigenvalues of C
            if abs(C(2,2)-lambda_1)<abs(C(2,2)-lambda_2)
                shift=lambda_1;
            else shift=lambda_2;
            end
        else w=s(iLower);
             C=diag(s)+diag(e,1);
             C=C*C';
             C=C(1:2,1:2);
             [lambda_1,lambda_2] = eig2(C);
             if abs(C(2,2)-lambda_1)<abs(C(2,2)-lambda_2)
                shift=lambda_1;
             else shift=lambda_2;
             end
        end
        if (shift/w)^2<=eps
            shift=0;
        end
    end
    % QR iterations
  if shift==0
        if dir==1
            [s,e]=ZeroShiftQR(s,e,iLower,iUpper,1); % Zero-shift QR iterations with direction down
            if e(iUpper-1)<=thresh
                e(iUpper-1)=0;
            end
        else [s,e]=ZeroShiftQR(s,e,iLower,iUpper,2); % Zero-shift QR iterations with direction up
            if e(iLower)<=thresh
                e(iLower)=0;
            end
        end
    else if dir==1
            [s,e]=ZeroShiftQR(s,e,iLower,iUpper,1); % This is where it should be applied the shifted QR dir. down
            if e(iUpper-1)<=thresh
                e(iUpper-1)=0;
            end
        else  [s,e]=ZeroShiftQR(s,e,iLower,iUpper,2); % This is where it should be applied the shifted QR dir. up
            if e(iLower)<=thresh
                e(iLower)=0;
            end
        end
    end     
end
% Sort singular values
s=sort(abs(s),'descend')