/*  -*- MaTX -*-
 *
 *  NAME
 *      lyap() - Solution of continous-time Lyapunov equation
 *
 *  SYNOPSIS
 *      P = lyap(A,Q)
 *         Matrix P;
 *         Matrix A,Q;
 *
 *      P = lyap(A,B,Q)
 *         Matrix P;
 *         Matrix A,B;
 *         Matrix Q;
 *
 *  DESCRIPTION
 *    	 lyap(A,Q) returns the solution of the continous-time Lyapunov 
 *       equation:
 *
 *    		A*P + P*A# = -Q
 *
 *    	 lyap(A,B,Q) returns the solution of the matrix equation:
 *
 *    		A*P + P*B = -Q
 *
 *  REFERENCES
 *      [1] Parlett's method. Golub and VanLoan (1983)
 *
 *	SEE ALSO
 *      dlyap
 */

Func Matrix Lyapunov(A, Q)
	Matrix A, Q;
{
	Matrix P;

	P = lyap(A, Q);
	return P;
}

Func Matrix lyap(A, B_, Q_, ...)
	Matrix A, B_, Q_;
{
	Integer i,n,nb;
	Matrix B,Q,P,Y;
	CoMatrix Ua,Ta,Ub,Tb;
	Index idx;
	Array Pa,Pb,PP;
	String msg;

	error(nargchk(2, 3, nargs, "lyap"));

	if (nargs == 2) {
		Q = B_;
		B = A#;

		if (length((msg = abcdchk(A))) > 0) {
			error("lyap(): " + msg);
		}
	} else {
		B = B_;
		Q = Q_;

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

	n = Rows(A);
	nb = Rows(B);

	if (nb != Cols(B) || Rows(Q) != n || Cols(Q) != nb) {
		 error("lyap(): Inconsistency in dimensions of arguments.\n");
	}

	{Ua,Ta} = schur(A);
	{Ua,Ta} = rsf2csf(Ua,Ta);
	{Ub,Tb} = schur(B);
	{Ub,Tb} = rsf2csf(Ub,Tb);

	// Check all combinations of diagonal elements of Ta and Tb
	Pa = ONE(nb,1)*diag_vec(Ta)#;
	Pb = diag_vec(Tb)*ONE(1,n);
	PP = abs(Pa) + abs(Pb);
	
	if (any(PP .== 0) || any(abs(Pa + Pb) < 1000*EPS*PP)) {
		error("lyap(): Solution is not unique.\n");
	}

	Q = -Ua# * Q * Ub;

	// 1st column
	Y = (Z(n,nb),:);
	Y(:,1) = (Ta .+ Tb(1,1)) \ Q(:,1);

	// 2nd column -- Rows(B)'th column
	for (i = 2; i <= nb; i++) {
		idx = [1:i-1];
		Y(:,i) = (Ta .+ Tb(i,i)) \ (Q(:,i) - Y(:,idx)*Tb(idx,i));
	}

	P = Ua*Y*Ub#; 

	if (isreal(A) && isreal(B_) && isreal(Q_)) {
		P = Re(P);
	}
	return P;
}