/* -*- MaTX -*-
 *
 *  NAME
 *      tfm2ss() - Transfer function matrix to state-space conversion
 *
 *  SYNOPSIS
 *      {A,B,C,D} = tfm2ss(G)
 *        Matrix A,B,C,D;
 *        RaMatrix G;
 *
 *      {A,B,C,D} = tfm2ss(G,iu)
 *        Matrix A,B,C,D;
 *        RaMatrix G;
 *        Integer iu;
 *
 *      {A,B,C,D} = tfm2ss(G,iu,tol)
 *        Matrix A,B,C,D;
 *        RaMatrix G;
 *        Integer iu;
 *        Real tol;
 *
 *  DESCRIPTION
 *       tfm2ss() returns the state-space representation:
 *           .
 *           x = Ax + Bu
 *           y = Cx + Du
 *
 *       of the system
 *                           -1
 *           G(s) = C (sI - A) B + D
 *
 *       tfm2ss(G,iu) returns the state-space representation from 
 *       the iu'th input.
 *
 *       tfm2ss() returns the minimal realiation by using minreal().
 *       tfm2ss(...,tol) passes tol to minreal().
 *
 *  SEE ALSO
 *      tfm2tf, tfm2tfn, tfm2zp, and ss2tfm
 */

Func List tfm2ss(G, iu, tol, ...)
	RaMatrix G;
	Integer iu;
	Real tol;
{
	Integer i,j,m,p,is,it;
	Matrix AA,BB,CC,DD;
	Matrix A,B,C,D,a,b,c,d;
	Matrix num,den;
	Rational g;

	error(nargchk(1, 3, nargs, "tfm2ss"));
	
	m = Cols(G);
	p = Rows(G);
	
	if (nargs == 1) {
		is = 1;
		it = m;
	} else {
		is = iu;
		it = iu;
	}
	
	for (i = is; i <= it; i++) {
		for (j = 1; j <= p; j++) {
			g = G(j,i);
			if (version() == 4) {
				num = fliplr(Matrix(Nu(g)));
				den = fliplr(Matrix(De(g)));
			} else {
				num = Matrix(Nu(g));
				den = Matrix(De(g));
			}
			if (length(num) < length(den)) {
				num = [Z(1,length(den)-length(num)), num];
			}

			{a,b,c,d} = tf2ss(num, den);

			if (nargs == 3) {
				{a,b,c,d} = minreal(a,b,c,d,tol);
			} else {
				{a,b,c,d} = minreal(a,b,c,d);
			}
			// a, b, c, d matrices are returned in controllable canonical form

			// Revised with the advice of Dr. Nonaka (1999.11.04)
			if (Rows(a) == 0) {
				// G(j,i) is constant
				{a,b,c,d} = {Z(0),Z(0,1),Z(1,0),d};
			}

			if (j == 1) {
				A = a;
				B = b;
				C = c;
				D = d;
			} else {
				A = diag(A,a);
				B = [[B]
					 [b]];
				C = diag(C,c);
				D = [[D]
					 [d]];
				if (nargs == 3) {
					{A,B,C,D} = minreal(A,B,C,D,tol);
				} else {
					{A,B,C,D} = minreal(A,B,C,D);
				}
			}
		}
		if (i == is) {
			AA = A;
			BB = B;
			CC = C;
			DD = D;
		} else {
			AA = diag(AA,A);
			BB = diag(BB,B);
			CC = [CC C];
			DD = [DD D];
			if (nargs == 3) {
				{AA,BB,CC,DD} = minreal(AA,BB,CC,DD,tol);
			} else {
				{AA,BB,CC,DD} = minreal(AA,BB,CC,DD);
			}
		}
	}

	return {AA,BB,CC,DD};
}