/* -*- MaTX -*-
 *
 *  NAME
 *      are() - Solution of continuous-time Riccati equation
 *
 *  SYNOPSIS
 *      P = are(A,R,Q)
 *         Matrix P;
 *         Matrix A,R,Q;
 *
 *      P = are(A,R,Q,tol)
 *         Matrix P;
 *         Matrix A,R,Q;
 *         Real tol;
 *
 *  DESCRIPTION
 *       are(A,R,Q) returns the stabilizing solution of the continuous-time 
 *       Riccati equation:
 *
 *         A#*P + P*A - P*R*P + Q = 0
 *
 *       by using the schur decomposition, where R is symmetric and 
 *       nonnegative definite and Q is symmetric.
 *
 *       are(...,tol) uses tol to determine the location of the poles
 *       with respect to the imaginary axis.
 *
 *  SEE ALSO
 *      ric, riccati, and lqr
 */

Func Matrix are(A,R,Q,tol, ...)
	Matrix A,R,Q;
	Real tol;
{
	Integer i,n,np;
	Complex j;
	String msg;
	Matrix P,idx;
	CoMatrix U,T;

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

	if (length((msg = abcdchk(A))) > 0) {
		error("are(): " + msg);
	}

	n = Rows(A);

	if (Rows(R) != n || Cols(R) != n) {
		error("are(): Dimensioned of R matrix is invalid.\n");
	}
	if (Rows(Q) != n || Cols(Q) != n) {
		error("are(): Dimensioned of Q matrix is invalid.\n");
	}

	j = (0,1);
	{U,T} = schur([[ A, -R ]
				   [-Q, -A#]]*(1.0+EPS*EPS*j)); 
	
	if (nargs < 4) {
		tol = 10.0*EPS*max(abs(diag2vec(T)));
	}

	/*
	 *  Location of eigenvalues:
	 *    -1 : open left-half-plane
	 *     1 : open right-half-plane
	 *     0 : within tolerance of imaginary axis
	 */
	np = 0;
	idx = [];
	for (i = 1; i <= 2*n; i++) {
		if (Re(T(i,i)) < -tol) {
			idx = [idx, -1];
			np++;
		} else if (Re(T(i,i)) > tol) {
			idx = [idx 1];
		} else {
			idx = [idx 0];
		}
	}

	if (np != n) {
		error("are(): No solution, (A,B) may be uncontrollable or no solution exists.\n");
	}

	{U,T} = schord(U,T,idx);
	P = Re(U(n+1:n+n,1:n)/U(1:n,1:n));

	return P;
}