/* -*- MaTX -*-
 *
 *  NAME
 *      minreal() - Minimal realization
 *
 *  SYNOPSIS
 *      {Am,Bm,Cm,Dm,tol} = minreal(A,B,C,D)
 *         Real tol;
 *         Matrix Am,Bm,Cm,Dm;
 *         Matrix A,B,C,D;
 *
 *      {Am,Bm,Cm,Dm,tol} = minreal(A,B,C,D,tol)
 *         Real tol;
 *         Matrix Am,Bm,Cm,Dm;
 *         Matrix A,B,C,D;
 *         Real tol;
 *
 *      {Am,Bm,Cm,Dm,tol} = minreal(A,B,C,D,tol,order)
 *         Real tol;
 *         Matrix Am,Bm,Cm,Dm;
 *         Matrix A,B,C,D;
 *         Real tol;
 *         Integer order;
 *
 *  DESCRIPTION
 *      minreal(A,B,C,D) returns a minimal realization of the system (A,B,C,D)
 *
 *      minreal(..., tol) uses the tolerance tol in deciding which states
 *      to be eliminated.
 *
 *      minreal(..., order) returns a realization of order 'order', 
 *      while the tolerance is automaticaly decided.
 *
 *  EXAMPLE
 *      s = Polynomial("s")
 *      G = (s+1)/((s+1)*(s+2));
 *      {A,B,C,D} = tfm2ss([G]);
 *      {Am,Bm,Cm,Dm} = minreal(A,B,C,D);
 *      print G,ss2tfm(Am,Bm,Cm,Dm);
 *
 *  SEE ALSO
 *      balreal
 */

Func List minreal(A, B, C, D, tol, order, ...)
	Matrix A, B, C, D;
	Real tol;
	Integer order;
{
	Integer flag;
	Matrix Am, Bm, Cm, Dm;
	String msg;

	List minreal_local();

	error(nargchk(4, 6, nargs, "minreal"));

	if (length((msg = abcdchk(A, B, C, D))) > 0) {
		error("minreal(): " + msg);
	}

	if (nargs == 6) {
		if (order < 1 || Rows(A) < order) {
			error("minreal(): order must be between 1 and Rows(A)\n");
		}
	} else {
		order = Rows(A);
	}

	if (nargs != 5) {
		tol = 10*Rows(A)*norm(A,1)*EPS;
	}

	flag = 0;
	do {
		{Am, Bm, Cm, Dm} = minreal_local(A, B, C, D, tol);
		if (nargs != 6 || Rows(Am) == order) {
			break;
		}
		if (order < Rows(Am)) {
			if (flag == 2) {
				break;
			}
			tol = tol*2;
			flag = 1;
		} else if (order > Rows(Am)) {
			if (flag == 1) {
				break;
			}
			tol = tol/2;
			flag = 2;
		}
	} while (1);

	return {Am,Bm,Cm,Dm,tol};
}

Func List minreal_local(A, B, C, D, tol)
	Matrix A, B, C, D;
	Real tol;
{
	Integer n, m, p, uc, uo, k;
	Matrix Am, Bm, Cm, Dm, T, K;

	n = Rows(A); // number of states of the given system
	m = Cols(B); // number of inputs
	p = Rows(C); // number of outputs
	
	{Am,Bm,Cm,T,K} = ctrf(A,B,C,tol);
	k = Integer(sum(K));
	if (k > 0) {
		uc = n - k; // number of uncontrollable mode
		Am = Am(uc+1:n,uc+1:n);
		Bm = Bm(uc+1:n,:);
		Cm = Cm(:,uc+1:n);
	} else {
		Am = Z(0,0);
		Bm = Z(0,m);
		Cm = Z(p,0);
	}

	n = n - uc; // number of controllable mode

	if (n > 0) {
		{Am,Bm,Cm,T,K} = obsf(Am,Bm,Cm,tol);
		k = Integer(sum(K));
		if (k > 0) {
			uo = n - k; // number of unobserbale mode
			Am = Am(uo+1:n,uo+1:n);
			Bm = Bm(uo+1:n,:);
			Cm = Cm(:,uo+1:n);
		} else {
			Am = Z(0,0);
			Bm = Z(0,m);
			Cm = Z(p,0);
		}
	}

	Dm = D;

	return {Am, Bm, Cm, Dm};
}