/* -*- MaTX -*-
 *
 *  NAME
 *      balreal() - Continous-time balanced realization
 *
 *  SYNOPSIS
 *      {Ab,Bb,Cb,G,T} = balreal(A,B,C)
 *         Matrix Ab,Bb,Cb,G,T;
 *         Matrix A,B,C;
 *
 *  DESCRIPTION
 *       balreal(A,B,C) returns a balanced realization of the given system.
 *
 *       balreal() also returns a vector G containing the diagonal of the
 *       gramian, and matrix T for the similarity transformation.  
 *
 *  SEE ALSO
 *      minreal and dbalreal
 */

Func List balreal(A,B,C)
	Matrix A,B,C;
{
	String msg;
	Matrix Ab,Bb,Cb,G,T;
	Matrix GG,Gc,Go,R,rGr,V,D;
	Index idx;

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

	Gc = gramian(A, B);
	Go = gramian(A#, C#);
	R = chol(Gc);
	rGr = R * Go * R#;
	rGr = tril(rGr) + tril(rGr, -1)#;

	{D,V} = eig(rGr);
	T = R# * V * vec2diag(diag2vec(D) .^ (-0.25));
	Ab = T \ A * T;
	Bb = T \ B;
	Cb = C * T;
	G = diag2vec(gramian(Ab, Bb))#;

	// G becomes in descending order
	{GG,idx} = sort(G);
	idx = fliplr(idx);
	Ab = Ab(idx,idx);
	Bb = Bb(idx,:);
	Cb = Cb(:,idx);

	T = T(idx,idx);
	G = G(idx);

	if (isreal(A) && isreal(B) && isreal(C)) {
		Ab = Re(Ab);
		Bb = Re(Bb);
		Cb = Re(Cb);
		T  = Re(T);
		G  = Re(G);
	}

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