## Copyright (C) 2017 Vasilis Lefkopoulos <vlefkopo@gmail.com>
##
## This program is free software; you can redistribute it and/or modify it under
## the terms of the GNU General Public License as published by the Free Software
## Foundation; either version 3 of the License, or (at your option) any later
## version.
##
## This program is distributed in the hope that it will be useful, but WITHOUT
## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
## details.
##
## You should have received a copy of the GNU General Public License along with
## this program; if not, see <http://www.gnu.org/licenses/>.

## FLAG = isstable (B, A)
## Returns a logical output, FLAG, equal to TRUE if the filter specified by 
## numerator coefficients B and denominator coefficients A is stable. Input
## vector A can also be an empty vector or, alternatively, completely ommited.
##
## FLAG = isstable (SOS)
## Returns a logical output, FLAG, equal to TRUE if the filter specified by the
## second order sections matrix SOS is stable.
##
## Example 1:
##   b = [1 2 3 4 5 5 1 2];
##   a = [4 5 6 7 9 10 4 6];
##   flag = isstable (b, a)
##
## Example 2:
##   [z, p, k] = butter (6, 0.7, 'high');
##   sos = zp2sos (z, p, k);
##   flag = isstable (sos)

function flag = isstable (b, varargin)
  
  if isempty(varargin)
    # Only one argument was given
    if any(size(b) > [1 1])
      # A matrix was given and is converted to vectors b & a
      [b,a] = sos2tf(b);
    else
      # Second input vector was omitted
      a = 1;
    endif  
  else
    a = varargin{1};
  endif

  if isempty(a) || (length(a) == 1)
    # An FIR filter is always stable
    flag = true;
  else
    if (a(1) != 1)
      # Normalization, so that a(1) equals 1
      b = b ./ a(1);
      a = a ./ a(1);
    endif
    r = roots(a);
    if any(abs(r) > 1)
      flag = false;
    else
      flag = true;
    endif
  endif

endfunction

%!test
%! b = [1 2 3 4 5 5 1 2];
%! a = [];
%! assert (isstable (b,a), true)

%!test
%! b = [1 2 3 4 5 5 1 2];
%! a = [4 5 6 7 9 10 4 6];
%! assert (isstable (b,a), false)

%!test
%! b = [1 2 3 4 5 5 1 2];
%! a = [4 5 6 7 9 10 4 6];
%! a = polystab(a);
%! assert (isstable (b,a), true)

%!test
%! [z,p,g] = butter(6,0.7,'high');
%! sos = zp2sos(z,p,g);
%! assert (isstable(sos) , true)