[a,b,c,d]=Utility_zpk2ss(z,p,k)

function [a,b,c,d]=Utility_zpk2ss(z,p,k)
% Utility_zpk2ss is a subfile of the AnalogFilter GUI collection
%
% James C. Squire, 2002
% Assistant Professor, Virginia Military Institute
% ver 1.0

% Utility_zpk2ss converts a zero/pole/gain formualation into a ss form
% Heavily modified from code in the Matlab Controls Toolbox

% Get number of outputs/inputs 
nz = length(z);
np = length(p);
z=z(:); p=p(:);
% Put complex pairs first
cp = p(imag(p)>0);  ncp = length(cp);
cz = z(imag(z)>0);  ncz = length(cz);
tp = zeros(np,1);   
tp([1:2:2*ncp 2:2:2*ncp 2*ncp+1:np]) = [cp ; conj(cp) ; p(~imag(p))];
tz = zeros(nz,1);   
tz([1:2:2*ncz 2:2:2*ncz 2*ncz+1:nz]) = [cz ; conj(cz) ; z(~imag(z))];
nsec2 = max(ncp,ncz);
nsec = np - nsec2;
% Realize first and second order sections
a = zeros(np);
b = zeros(np,nsec);
c = zeros(nsec,np);
d = zeros(nsec,1);
% Balanced realizations of second-order sections
for j=1:nsec2,
    xr = [2*j-1,2*j];
    ttz = tz(xr(1):min(xr(2),nz),:);
    ttp = tp(xr);
    switch length(ttz)
        case 0, ttd = 0;  tte = 0;
        case 1, ttd = 0;  tte = 1;
        case 2, ttd = 1;  tte = -real(sum(ttz));
    end
    rp = real(ttp);    ip = imag(ttp(1));
    if abs(ip)>=1
        a12 = ip;  a21 = -ip;
    else
        a12 = 1;   a21 = -ip^2;
    end
    x = ttd*sum(rp)+tte; 
    y = (real(prod(rp(2)-ttz))-ttd*ip^2)/a12; 
    lambda = (x^2+y^2)^0.25;
    a(xr,xr) = [rp(1) , a12 ; a21 , rp(2)];
    if lambda
        b(xr,j) = [x ; y]/lambda;
    else
        b(xr,j) = [0;0];
    end
    c(j,xr) = [lambda , 0];
    d(j) = ttd;
end
% Balanced realizations of first-order sections
for j=1:nsec-nsec2,
    xr = 2*nsec2+j;
    jsec = nsec2+j;
    ttz=tz(xr:min(xr,nz),:);
    ttp=tp(xr);
    nump = prod(ttp-ttz);     
    lambda = sqrt(abs(nump)); 
    a(xr,xr) = ttp;
    b(xr,jsec) = lambda;
    c(jsec,xr) = sign(nump)*lambda;
    d(jsec) = (length(ttz)~=0);
end
% Put it all together
idmc = (eye(nsec) - diag(d(1:nsec-1,:),1))\c;
imd = eye(nsec) - diag(d(2:nsec,:),1); 
ks = sqrt(abs(k));
a = a + b(:,1:nsec-1) * idmc(2:nsec,:);
b = b * (imd\[zeros(nsec-1,1);1])*ks;
c = idmc(1,:)*(k/ks);
d = all(d)*k;
tiny = 1e-32;
condt = 1/eps;
nx = size(a,1);
mae = abs(a);
mb = abs(b);
mc = abs(c);
bmax = max(mb); 
cmax = max(mc);
tiny = max(1e-100,(1/condt)^2);
mb(~mb) = tiny * bmax + (bmax==0)*cmax;
mc(~mc) = tiny * cmax + (cmax==0)*bmax;
nx = size(mae,1);
idx = 1:nx;  
idx = idx+nx*(idx-1);  % indices for diagonal elements
ndiag = max(mae(idx));   
mae(idx) = 0;   % zero out diagonal
[t0,a0] = balance(mae);
na0 = max(abs(a0(:))); 
sfio = 1;
iter = 0;
while iter<5,
    [t,m] = balance([mae mb;mc 0]);
    sfx = max(t(1:nx,:),[],2); 
    sfbc = max(t(nx+1,:));     
    na = ndiag + min(na0,max(max(m(1:nx,1:nx))));
    nbc = max(max(m(:,nx+1)),max(m(nx+1,:)));
    na = na + (~na) * nbc;  % protection against na=0
    if nbc<na/2 | nbc>16*na,
        sf = pow2(round(log2(na/nbc)+1));
    else
        break
    end
    mb = mb*sf;  mc = mc*sf;  sfio = sfio*sf;
    iter = iter+1;
end
if isfinite(condt) & max(sfx)>10*condt*min(sfx),
   sfx = log2(sfx);
   scalf = log2(condt)/(max(sfx)-min(sfx));
   sfx = pow2(round(scalf*sfx));
end
isfx = 1./sfx;
sfx = sfx.';
a = (isfx(:,ones(1,nx)) .* a) .* sfx(ones(1,nx),:);
b = (isfx .* b) * sfbc;
c = (c .* sfx) / sfbc;