/* -*- MaTX -*-
 *
 *  NAME
 *      ss2zp()	- State-space to zero-pole conversion
 *
 *  SYNOPSIS
 *      {z,p,k} = ss2zp(A,B,C,D)
 *         CoMatrix z,p;
 *         Matrix k;
 *         Matrix A,B,C,D;
 *
 *      {z,p,k} = ss2zp(A,B,C,D,i)
 *         CoMatrix z,p;
 *         Matrix k;
 *         Matrix A,B,C,D;
 *         Integer i;
 *
 *  DESCRIPTION
 *       ss2zp() returns the zeros z, poles p, and gains k of the system
 *          .
 *          x = Ax + Bu
 *          y = Cx + Du
 *
 *       whose transfer function is given by
 *
 *                         -1          (s-z1)(s-z2)...(s-zn)
 *      	G(s) = C(sI-A) B + D =  k ---------------------
 *                                     (s-p1)(s-p2)...(s-pn)
 *
 *       ss2zp(...,i) calculates those values from the i'th input.
 *
 *  SEE ALSO
 *      ss2tf, ss2tfn, ss2tfm, and zp2ss
 */

Func List ss2zp(A,B,C,D,i, ...)
	Matrix A,B,C,D;
	Integer i;
{
	Integer j, n, p, nz;
	Matrix k, Bi, Di, CA;
	CoMatrix po, zz, z;
	Index idx;
	String msg;

	error(nargchk(4, 5, nargs, "ss2zp"));

	if (length((msg = abcdchk(A, B, C, D))) > 0) {
		error("ss2zp(): " + msg);
	}
	
	if (nargs == 4) {
		i = 1;
	}

	n = Rows(A);
	p = Rows(C);

	Bi = B(:,i);
	Di = D(:,i);

	// zeros
	z = (ONE(n,p),:)*Inf;
	for (j = 1; j <= p; j++) {
		zz = eigval([[  A      Bi  ]
					 [C(j,:), Di(j)]], diag(I(n), 0));

		zz = zz( ! isnan(zz) && isfinite(zz));
		if ((nz = length(zz))) {
			z(1:nz,i) = zz;
		}
	}

	// gains
	k = Di;
	CA = C;
	j = 0;
	while (any(k .== 0)) {
		idx = find(k .== 0);
		if (j > n) {
			k(idx) = Z(length(idx),1);
			break;
		}
		k(idx) = [CA*Bi](idx);
		CA = CA*A;
		j++;
	}

	// poles
	po = eigval(A);

	return {z,po,k};
}