% [a, b, p, q, kstr] = guptri2 (a0, b0, tol0, plane)
%
% Finds p and q such that a = p'*a0*q and b = p'*b0*q are on a
% staircase form
%
% The algorithm for finding the transformation is as follows:
%
% 1) Find L_j and J_j(0) blocks by performing an SVD on a0 to 
% so that a0*q = [0 a2]. Perform the same transformation on b0
% so that b0*q = [b1 b2]. Next perform an SVD on b1 so that
% p'*b1 = [b1a; 0]. We have then obtained the following
% block-triangular partitioning
% p'*a0*q = [0 a2a;
%           [0 a2b];   and p'*b0*q = [b1a b2a;
%                                    [0   b2b]
% Continue by performing the same partioning on a2b and b2b
% until no zero blocks are found.
%
% The block structure now reveals the blocks L_j and J_j(0)
%
% 2) Use the same algorithm as in 1) on the result from it but
% swap anb and bnb. This step picks out N_j blocks. No L_j blocks
% should be found, since these are already detected in step 1)
%
% 3) This step is similar to the first step but it is applied to
% anb' and bnb' obtained from step 2). We also reverse the order of 
% the columns and rows, so that zero blocks are located to the lower
% right corner. This step will pick out any L_j' blocks.
%
% 4) What now remains is a square J_j(µ) block, which contains all
% ordinary eigenvalues excluding 0 and infty. This may contain Jordan
% blocks but this is not detected in the present implementation.

function [a, b, p, q, kstr] = guptri2 (a0, b0, tol0, plane)

if nargin < 2,
  disp ('usage:  [a, b, p, q, kstr] = guptri2 (a0, b0, tol, plane)');
  return;
elseif nargin < 3,
  tol0 = 1e-12;
end

if nargin < 4, plane = 'lhp'; end

tol = tol0;
% normalize a0 and b0
if norm (a0, 'fro') > 0,
  an = a0/norm (a0, 'fro');
else
  an = a0;
end
if norm (b0, 'fro') > 0,
  bn = b0/norm (b0, 'fro');
else
  bn = b0;
end

n1 = size(a0,2);
n2 = size(a0,1);

% find L and J(0) blocks
[a, b, p, q, kstr, tol] = svdstep (an, bn, tol);
if isempty (kstr),
  kstr = [0; 0];
end
nn = sum (kstr, 2);

a1 = a(nn(2)+1:end,nn(1)+1:end);
b1 = b(nn(2)+1:end,nn(1)+1:end);
if kstr(:,1) == [0; 0], kstr = kstr(:,2:end); end
kstr = [kstr [-1; -1]];

% find N blocks by swapping a and b
[b2, a2, p2, q2, kstr2, tol] = svdstep (b1, a1, tol);

p(:,nn(2)+1:end) = p(:,nn(2)+1:end)*p2;
q(:,nn(1)+1:end) = q(:,nn(1)+1:end)*q2;

if isempty (kstr2),
  kstr2 = [0; 0];
end
nn2 = sum (kstr2, 2);

a2 = a2(nn2(2)+1:end,nn2(1)+1:end);
b2 = b2(nn2(2)+1:end,nn2(1)+1:end);

% reverse the order of columns and rows in a2 and b2
if kstr2(:,1) == [0; 0], kstr2 = kstr2(:,2:end); end
kstr = [kstr kstr2 [-1; -1]];

% find L' and N blocks by swapping (a,b) and (a', b') and q with p
if sum (size(a2)) > 0,
  [a3, b3, q3, p3, kstr3, tol] = svdstep (a2(end:-1:1,end:-1:1)', ...
                                          b2(end:-1:1,end:-1:1)', tol);
  a = p'*an*q;
  b = p'*bn*q;

  p(:,nn(2)+nn2(2)+1:end) = p(:,nn(2)+nn2(2)+1:end)*p3(end:-1:1,end:-1:1);
  q(:,nn(1)+nn2(1)+1:end) = q(:,nn(1)+nn2(1)+1:end)*q3(end:-1:1,end:-1:1);

  a = p'*an*q;
  b = p'*bn*q;

  if isempty (kstr3),
    nn2 = [0; 0];
  else
    nn3 = sum(kstr3([2 1],:),2);
  end
  if kstr3(:,1) == [0; 0], kstr3 = []; end
  kstr = [kstr kstr3 [-1; -1]];
  kk3 = [n1;n2] - nn - nn2 - nn3;
else
  kk3 = [n1;n2] - nn - nn2;
  kstr = [kstr [-1; -1]];
end

if kk3 ~= [0; 0],

  % check that the last block, which contains "normal" eigenvalues,
  % is square. Then we need to order the eigenvalues so that their 
  % real parts are increasing

  if kk3(1) ~= kk3(2),
    error ('internal error: last block must be square');
  end
  a3 = a(nn(2)+nn2(2)+(1:kk3(2)),nn(1)+nn2(1)+(1:kk3(1)));
  b3 = b(nn(2)+nn2(2)+(1:kk3(2)),nn(1)+nn2(1)+(1:kk3(1)));

  % note that qz (a2, b2, 'complex') produces complex matrices
  % when the eigenvalues are complex. The 'complex' option is
  % default. Using the 'real' option produces a block triangular
  % structure.

  [a4, b4, p4, q4] = qz (a3, b3, 'real');
  ee = ordeig (a4, b4);
  if strcmp (plane, 'lhp')
    select = ee < tol;
  else
    select = ee > -tol;
  end
  [a5, b5, p5, q5] = ordqz (a4, b4, p4, q4, select);

  q(:,nn(1)+nn2(1)+(1:kk3(1))) = q(:,nn(1)+nn2(1)+(1:kk3(1)))*q5;
  p(:,nn(2)+nn2(2)+(1:kk3(1))) = p(:,nn(2)+nn2(2)+(1:kk3(1)))*p5';
  kstr = [kstr kk3];
end
kstr = [kstr [-1; -1]];

% compute a and b on original a0 and b0
a = p'*a0*q;
b = p'*b0*q;

% zero elements in a and b below staircase

n1 = 0;
n2 = 0;
i = 1;

b(:,1:kstr(1,i)-kstr(2,i)) = 0;
while kstr(1,i) > 0,
  a(n2+1:end,n1+(1:kstr(1,i))) = 0;
  n1 = n1 + kstr(1,i);
  n2 = n2 + kstr(2,i);
  b(n2+1:end,1:n1) = 0;
  i = i + 1;
end

if kstr(1,i) == 0, i = i + 1; end
i = i + 1;

a(:,1:kstr(1,i)-kstr(2,i)) = 0;
while kstr(1,i) > 0,
  b(n2+1:end,n1+(1:kstr(1,i))) = 0;
  n1 = n1 + kstr(1,i);
  n2 = n2 + kstr(2,i);
  a(n2+1:end,1:n1) = 0;
  i = i + 1;
end

if kstr(1,i) == 0, i = i + 1; end
i = i + 1;
[n2, n1] = size (a);
while kstr(1,i) > 0,
  a(n2-kstr(1,i)+1:end,1:n1) = 0;
  b(n2-kstr(1,i)+kstr(2,i)+1:end,1:n1) = 0;
  n2 = n2 - kstr(1,i);
  n1 = n1 - kstr(2,i);
  i = i + 1;
end

if kstr(1,i) == 0, i = i + 1; end
i = i + 1;
while kstr(1,i) > 0,
  % regular block
  i = i + 1;
end

% check error norm
err = norm ([norm(p'*a0*q-a,'fro')/norm(a0,'fro');
             norm(p'*b0*q-b,'fro')/norm(b0,'fro')]);

if err > 1.5*tol0, fprintf ('large tolerance : %8.2e\n', err); end

if nargout < 4, p = kstr; end
if nargout < 2, a = kstr; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [a, b, p, q, kstr, tol] = svdstep (a0, b0, tol, verb)

if nargin < 3,
  disp ('usage:  [a, b, p, q, kstr, tol] = svdstep (a0, b0, tol)');
  return;
end

if nargin < 4,
  verb = 0;
end

a = a0;
b = b0;
[kstr1, kstr2] = size (a0);
p = eye (kstr1);
q = eye (kstr2);
kstr = [];

if kstr1 == 0,
  if kstr2 > 0,
    kstr = [kstr2; 0];
  end
  return;
end

[u, s, v] = svd (a0);
[n1, tol] = selectorder (s, tol, verb);
q = v(:,end:-1:1);
a = a0*q;
b = b0*q;
k1 = size (a0,  2) - n1;
% a = p'*a0*q
if k1 > 0,
  [p, s, v] = svd (b(:,1:k1));
  [k2, tol] = selectorder (s, tol, verb);
  q2 = v(:,[k1:-1:k2+1 1:k2]);
  a = p'*a;
  b = p'*b;
  a(:,1:k1) = a(:,1:k1)*q2;
  b(:,1:k1) = b(:,1:k1)*q2;
  q(:,1:k1) = q(:,1:k1)*q2;
else
  k2 = 0;
end
kstr = [k1; k2];

if k2 < 1,
  return;
elseif sum (size(a0)-kstr') > 0,
  [a1, b1, p1, q1, kstr1, tol] = svdstep (a(k2+1:end,k1+1:end), ...
                                          b(k2+1:end,k1+1:end), ...
                                          tol);
  q(:,k1+1:end) = q(:,k1+1:end)*q1;
  p(:,k2+1:end) = p(:,k2+1:end)*p1;
  kstr = [kstr kstr1];
  a = p'*a0*q;
  b = p'*b0*q;
else
  kstr = [kstr [size(a0,2)-k1; size(a0,1)-k2]];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute number of columns/rows to keep, k, given an tolerance, tol0

function [k, tol] = selectorder (s0, tol0, verb)
tol = tol0;
if min (size(s0)) == 0,
  k = 0;
elseif min (size(s0)) > 1,
  s2 = sqrt (cumsum (sort (diag (s0)).^2));
  k = sum (s2 > tol0);
  if length(s2) > k,
    if verb > 0, fprintf ('%d choices\n',length(s2) - k + 1); end
    tol = sqrt (tol0^2 - s2(end-k)^2);
  end
elseif s0(1,1) > tol0,
  k = 1;
else
  if verb > 0, disp ('2 choices'); end;
  tol = sqrt (tol0^2 - s0(1,1)^2);
  k = 0;
end
