/*  -*- MaTX -*-
 *
 *  NAME
 *      canon() - Canonical form transformation
 *
 *  SYNOPSYS
 *      {Ab,Bb,Cb,Db,T} = canon(A,B,C,D)
 *        Matrix Ab,Bb,Cb,Db,T;
 *        Matrix A,B,C,D;
 *
 *      {Ab,Bb,Cb,Db,T} = canon(A,B,C,D,type)
 *        Matrix Ab,Bb,Cb,Db,T;
 *        Matrix A,B,C,D;
 *        String type;
 *
 *  DESCRIPTION
 *      canon(A,B,C,D,"type") transforms the system (A,B,C,D) into the 
 *      canonical form specified by type:
 *
 *             "modal" : modal form where the system eigenvalues appear 
 *                       on the diagonal
 *
 *        "observable" : observable canonical form where the characteristic 
 *                       polynomial appears in the right column
 *
 *      "controllable" : controllable canonical form where the characteristic 
 *                       polynomial appears in the last row
 *
 *      T is the matrix for the transformation, x = Tz
 *
 *      CoCanTrans() is obsolete.
 *      Use  {Ac,bc,cc,dc} = canon(A,b,c,d,"controllable")
 *
 *  SEE ALSO
 *      ctrbm, ctrbf, obsv, and obsvf
 */

Func List canon(A,B,C,D,type, ...)
	Matrix A,B,C,D;
	String type;
{
	Integer k,n,nt;
	String msg;
	Matrix Ab,Bb,Cb,Db,T,V,Vi;
	CoMatrix val,vec;

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

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

	if (nargs == 4) {
		type = "modal";
	}

	n = Rows(A);
	nt = length(type);
  
	if (type == "modal"(1:min(5,nt))) {
		{val, vec} = eig(A);
		k = 1;
		T = [];
		while (k <= n) {
			if (Im(val(k,k)) != 0.0) {
				T(:,k)   = Re(vec(:,k));
				T(:,k+1) = Im(vec(:,k));
				k = k + 2;
			} else {
				T(:,k) = vec(:,k);
				k = k + 1;
			}
		}
		Ab = T \ A * T;
		Bb = T \ B;
		Cb = C * T;
		Db = D;
	} else if (type == "observable"(1:min(9,nt))) {
		if (length(B)) {
			T = ctrm(A,B(:,1));
			if (rank(T) < n) {
				error("canon(): System is uncontrollable from first input.\n");
			}
		} else {
			T = [];
		}
		Ab = T \ A * T;
		Bb = T \ B;
		Cb = C * T;
		Db = D;
	} else if (type == "controllable"(1:min(12,nt))) {
		if (length(B)) {
			V = ctrm(A,B(:,1));
			if (rank(V) < n) {
				error("canon(): System is uncontrollable from first input.\n");
			}
			Vi = inv(V);
			T = inv(obsm(A,Vi(n,:)));
		} else {
			T = [];
		}
		Ab = T \ A * T;
		Bb = T \ B;
		Cb = C * T;
		Db = D;
	} else {
		error("canon(): type must be either \"modal\", \"observable\" or \"controllable\".\n");
	}
	
	return {Ab,Bb,Cb,Db,T};
}

Func List CoCanTrans(A, b, c)
	Matrix A, b, c;
{
	Matrix Ac, bc, cc;

	{Ac,bc,cc} = canon(A,b,c,Z(1),"controllable");
	return {Ac, bc, cc};
}