/* -*- MaTX -*-
 *
 *  NAME
 *      tf2ss() -  Transfer function to state-space conversion
 *
 *  SYNOPSIS
 *      {A,B,C,D} = tf2ss(NUM,den)
 *          Matrix NUM,den;
 *          Matrix A,B,C,D;
 *
 *  DESCRIPTION
 *       tf2ss() returns the state-space representation:
 *           .
 *           x = Ax + Bu
 *           y = Cx + Du
 *
 *       of the SIMO system:
 *
 *                  NUM(s) 
 *          G(s) = --------
 *                  den(s)
 *
 *       den contains the coefficients of the denominators and 
 *       NUM contains the numerator coefficients 
 *
 *       The A,B,C,D matrices are returned in controller canonical form.
 *
 *  SEE ALSO
 *      tf2zp, tf2tfn, tf2tfm, and ss2tf
 */

Func List tf2ss(NUM_, den_)
	Matrix NUM_, den_;
{
	Integer n,nn;
	Matrix A,B,C,D,NUM,den;
	Index idx;

	NUM = NUM_;
	den = den_;

	// Strip leading zeros
	idx = find(den .!= 0);
	if (length(idx) > 0) {
		den = den(idx(1):length(den));
	} else {
		den = [1];
	}

	n = length(den);
	nn = Cols(NUM);

	if (nn > n) {
		// Strip leading zeros
		if (all(NUM(:,1:nn-n) .== 0)) {
			NUM = NUM(:,nn-n+1:nn);
			nn = Cols(NUM);
		} else {
			error("tf2ss(): Order of numerator > order of denominator.\n");
		}
	}

	// Pad numerator with leading zeros and normalize
	NUM = [Z(Rows(NUM),n-nn), NUM]/den(1);
	D = NUM(:,1);

	if (n == 1) {
		A = [];
		B = [];
		C = [];
		return {A,B,C,D};
	}

	den = den(2:n)/den(1);
	A = [[   -den   ]
		 [I(n-2,n-1)]];
	B = I(n-1,1);
	C = NUM(:,2:n) - NUM(:,1)*den;

	return {A,B,C,D};
}