/* -*- MaTX -*-
 *
 *  NAME
 *      ctrf() - Controllable-uncontrollable decomposition
 *
 *  SYNOPSIS
 *      {Ab,Bb,Cb,T,K} = ctrf(A,B,C)
 *          Matrix Ab,Bb,Cb,T,K;
 *          Matrix A,B,C;
 *
 *      {Ab,Bb,Cb,T,K} = ctrf(A,B,C,tol)
 *          Matrix Ab,Bb,Cb,T,K;
 *          Matrix A,B,C;
 *          Real tol;
 *
 *  DESCRIPTION
 *       ctrf() returns a decomposition into the controllable/
 *       uncontrollable subspaces.
 *
 *       If rank([A B]) < Rows(A), then there is a similarity
 *       transformation  z = T x  such that
 *
 *         Ab = T*A*T# ,  Bb = T*B ,  Cb = C*T#
 *
 *       and the transformed system has the form
 *
 *              [Anc| 0]       [ 0]
 *         Ab = [---+--], Bb = [--],  Cb = [Cnc|Cc].
 *              [A21|Ac]       [Bc]
 *                                                  -1           -1
 *       where (Ac,Bc) is controllable, and Cc(sI-Ac)Bc = C(sI-A)B.
 *
 *       ctrf(..., tol) uses tolerance tol to determine the order of
 *       the subspaces.
 *
 *  SEE ALSO
 *      ctrm and obsf
 */

Func List ctrf(A, B, C, tol, ...)
	Matrix A, B, C;
	Real tol;
{
	Integer i,n,d,s,r;
	Matrix Ab, Bb, Cb, T, k;
	Matrix An, Bn, BB, Au;
	Matrix P, U, D, V;
	String msg;

	error(nargchk(3, 4, nargs, "ctrf"));

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

	n = Rows(A);
	k = Z(1,n);

	if (nargs == 3) {
		tol = n * norm(A,1) * EPS;
	}

	d = 0;
	P = I(n);
	An = A;
	Bn = B;
	for (i = 1; i <= n; i++) {
		{U,D,V} = svd(Bn);
		U = U * fliplr(I(Rows(D)));
		BB = U# * Bn;

		k(i) = r = rank(BB,tol);
		s = Rows(BB) - r;

		if (r == 0 || s == 0) {
			break;
		}

		Au = U# * An * U;
		An = Au(1:s,1:s);
		Bn = Au(1:s,s+1:s+r);
		P = P * diag(U, I(d));
		d = d + r; 
	}

	T = P#;
	Ab = T*A*T#;
	Bb = T*B;
	Cb = C*T#;

	return {Ab, Bb, Cb, T, k};
}