## Copyright (C) 2005 Julius O. Smith III <jos@ccrma.stanford.edu>
##
## 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; see the file COPYING. If not, see
## <https://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn  {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z})
## @deftypefnx {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}, @var{p})
## @deftypefnx {Function File} {[@var{sos}, @var{g}] =} zp2sos (@var{z}, @var{p}, @var{k})
## @deftypefnx {Function File} {@var{sos} =} zp2sos (@dots{})
## Convert filter poles and zeros to second-order sections.
##
## INPUTS:
## @itemize
## @item
## @var{z} = column-vector containing the filter zeros
## @item
## @var{p} = column-vector containing the filter poles
## @item
## @var{k} = overall filter gain factor
## If not given the gain is assumed to be 1.
## @end itemize
##
## RETURNED:
## @itemize
## @item
## @var{sos} = matrix of series second-order sections, one per row:
## @example
## @var{sos} = [@var{B1}.' @var{A1}.'; ...; @var{BN}.' @var{AN}.']
## @end example
## where
## @code{@var{B1}.' = [b0 b1 b2] and @var{A1}.' = [1 a1 a2]} for
## section 1, etc.  The b0 entry must be nonzero for each section.
## See @code{filter} for documentation of the second-order direct-form filter
## coefficients @var{B}i and %@var{A}i, i=1:N.
##
## @item
## @var{g} is the overall gain factor that effectively scales
## any one of the @var{B}i vectors.
## @end itemize
##
## If called with only one output argument, the overall filter gain is
## applied to the first second-order section in the matrix @var{sos}.
##
## EXAMPLE:
## @example
##   [z, p, k] = tf2zp ([1 0 0 0 0 1], [1 0 0 0 0 .9]);
##   [sos, g] = zp2sos (z, p, k)
##
## sos =
##    1.0000    0.6180    1.0000    1.0000    0.6051    0.9587
##    1.0000   -1.6180    1.0000    1.0000   -1.5843    0.9587
##    1.0000    1.0000         0    1.0000    0.9791         0
##
## g =
##     1
## @end example
##
## @seealso{sos2pz, sos2tf, tf2sos, zp2tf, tf2zp}
## @end deftypefn

function [sos,g] = zp2sos(z,p,k)

  if nargin<3, k=1; endif
  if nargin<2, p=[]; endif

  [zc,zr] = cplxreal(z(:));
  [pc,pr] = cplxreal(p(:));

  ## zc,zr,pc,pr

  nzc=length(zc);
  npc=length(pc);

  nzr=length(zr);
  npr=length(pr);

  ## Pair up real zeros:
  if nzr
    if mod(nzr,2)==1, zr=[zr;0]; nzr=nzr+1; endif
    nzrsec = nzr/2;
    zrms = -zr(1:2:nzr-1)-zr(2:2:nzr);
    zrp = zr(1:2:nzr-1).*zr(2:2:nzr);
  else
    nzrsec = 0;
  endif

  ## Pair up real poles:
  if npr
    if mod(npr,2)==1, pr=[pr;0]; npr=npr+1; endif
    nprsec = npr/2;
    prms = -pr(1:2:npr-1)-pr(2:2:npr);
    prp = pr(1:2:npr-1).*pr(2:2:npr);
  else
    nprsec = 0;
  endif

  nsecs = max(nzc+nzrsec,npc+nprsec);

  ## Convert complex zeros and poles to real 2nd-order section form:
  zcm2r = -2*real(zc);
  zca2 = abs(zc).^2;
  pcm2r = -2*real(pc);
  pca2 = abs(pc).^2;

  sos = zeros(nsecs,6);
  sos(:,1) = ones(nsecs,1); # all 2nd-order polynomials are monic
  sos(:,4) = ones(nsecs,1);

  nzrl=nzc+nzrsec; # index of last real zero section
  nprl=npc+nprsec; # index of last real pole section

  for i=1:nsecs

    if i<=nzc # lay down a complex zero pair:
      sos(i,2:3) = [zcm2r(i) zca2(i)];
    elseif i<=nzrl # lay down a pair of real zeros:
      sos(i,2:3) = [zrms(i-nzc) zrp(i-nzc)];
    endif

    if i<=npc # lay down a complex pole pair:
      sos(i,5:6) = [pcm2r(i) pca2(i)];
    elseif i<=nprl # lay down a pair of real poles:
      sos(i,5:6) = [prms(i-npc) prp(i-npc)];
    endif
  endfor

  ## If no output argument for the overall gain, combine it into the
  ## first section.
  if (nargout < 2)
    sos(1,1:3) *= k;
  else
    g = k;
  endif

endfunction

%!test
%! B=[1 0 0 0 0 1]; A=[1 0 0 0 0 .9];
%! [z,p,k] = tf2zp(B,A);
%! [sos,g] = zp2sos(z,p,k);
%! [Bh,Ah] = sos2tf(sos,g);
%! assert({Bh,Ah},{B,A},100*eps);
