function [a,e,p,q] = hessperm(a,e)
%HESSPERM  Permute to block upper triangular form.
%
%   When A-s*E is not already in lower or upper Hessenberg form, HESSPERM
%   permutes A or (A,E) to block upper triangular form to minimize the work 
%   needed to reduce A or (A,E) to Hessenberg form. This is used as 
%   pre-processing for fast frequency response computation. HESSPERM
%   generalizes the dgebal/dggbal permutation in LAPACK and can be seen
%   as a way to limit the scope of numerical errors introduced by the 
%   Hessenberg reduction. In particular, upper triangular portions of
%   A-s*E are left untouched by the subsequent Hessenberg reduction. 
%
%   [AA,EE,p,q] = HESSPERM(A,E) returns AA=A(p,q) and EE=E(p,q).
%
%   [AA,~,p] = HESSPERM(A) assumes E=I and returns AA=A(p,p).

%   Authors: P. Gahinet
%   Copyright 2020 The MathWorks, Inc.

% RE: Expects A,E to be matrices 2D arrays, dense or sparse
if nargin<2
   e = [];
end
nx = size(a,1);
if nx<3
   p = (1:nx)';  q = p;  return
end

% Check if A-s*E is already in lower or upper Hessenberg form (in which
% case Hessenberg reduction is error free)
if isempty(e)
   mae = abs(a) + eye(nx,'like',a);
else
   mae = abs(a) + abs(e);
end

[lower,upper] = bandwidth(mae);
if lower<2
   % upper H
   p = (1:nx)';  q = p;
else
   if upper<2
      % lower H
      p = (nx:-1:1)';  q = p;
   else
      % Permute to block upper triangular
      % Note: p=q when e=[] (diagonal of MAE is nonzero)
      [p,q] = dmperm(mae);
      p = p(:);  q = q(:);
   end
   a = a(p,q);
   if ~isempty(e)
      e = e(p,q);
   end
end