function A = fstab(A,T,thresh,IsMatrix)
%FSTAB Stabilize monic polynomial or square matrix A.
%
%  Polynomial stabilization:
%   B = FSTAB(A, Ts) performs stabilization of monic polynomial A with
%   respect to the unit circle or imaginary axis. A must be specified as a
%   row vector. The output poynomial B is the stabilized monic polynomial.
%   Use Ts>0 if polynomial A must be stabilized in discrete-time domain:
%   roots whose magnitudes are greater than one are reflected inside the
%   unit circle.
%   Set Ts=0 to stabilize the polynomial in continuous-time domain: roots
%   whose real parts are greater than zero (right half plane roots) are
%   reflected in the left half plane. Ts is assumed to be 1 if not
%   specified.
%
%  Matrix stabilization:
%   B = FSTAB(A, Ts, [], 1). Eignevalues outside (right of) stability boundary
%   are reflected inside (left of) it.
%
%  B = FSTAB(A, Ts, THRESH, ...) allows specification of threshold for stability
%  checks. Default value of THRESH is 1 if Ts>0 and 0 if Ts==0.

%   L.Ljung 2-10-92
%	 Copyright 1992-2017 The MathWorks, Inc.

% REVISIT: threshold is modified (2*thresh when Ts~=0, or thresh^2 when
% Ts==0); Why is this so? Not consistent across various stabilization
% methods (fstab, oestab, n4sid/stab etc)
%
% Also REVISIT in fstabPolynomialRoots

ni = nargin;
narginchk(1,4)

if isempty(A), return, end
if ni<2, T = 1; end
if ni<3 || isempty(thresh), thresh = double(T~=0); end
if nargin<4, IsMatrix = ~isvector(A); end

if ~IsMatrix
   A = localStabilizePoly(A,T,thresh);
else
   A = localStabilizeMatrix(A,T,thresh);
end

%--------------------------------------------------------------------------
function A = localStabilizePoly(A,T,thresh)
% Stabilize a polynomial.

if isscalar(A), return, end
RealA = isreal(A);

v = roots(A);
[v,wasStable] = idpack.fstabPolynomialRoots(v,T,thresh);
if wasStable
    return; % roots in v were already stable, quick return
end

bv  = poly(v);

lbv = length(bv);
if T~=0
   A(1:lbv) = bv;
else
   A(end-lbv+1:end) = bv;
end

if RealA, A = real(A); end

%--------------------------------------------------------------------------
function A = localStabilizeMatrix(A,T,thresh)
% Stabilize a matrix.

RealA = isreal(A);
[V,D] = eig(A);
if cond(V)>10^8
   [V,D] = schur(A);
   [V,D] = rsf2csf(V,D);
end

if (T~=0 && max(abs(diag(D)))<thresh) || (T==0 && max(real(diag(D)))<thresh)
   return
end

[~,n] = size(D);
if T~=0
   for kk = 1:n
      if abs(D(kk,kk))>thresh, D(kk,kk) = thresh^2/D(kk,kk); end
   end
else
   for kk = 1:n
      if real(D(kk,kk))>thresh
         D(kk,kk) = 2*thresh - real(D(kk,kk)) + 1i*imag(D(kk,kk));
      end
   end
end

A = V*D/V;

if RealA, A = real(A); end
