/* -*- MaTX -*-
 *
 *  NAME
 *      hinf() - Solve H-infinity problem
 *
 *  SYNOPSIS
 *      Gc = hinf(G, n, mw, pz, gamma)
 *         Matrix Gc;
 *         Matrix G;
 *         Integer n, mw, pz;
 *         Real gamma;
 *
 *  DESCRIPTION
 *      Given the generalized plant
 *
 *           .            n  mw  mu
 *        [[ x ]    n  [[ A  B1  B2  ] [[ x ]      [[ x ]
 *         [ z ]  = pz  [ C1 D11 D12 ]  [ w ]  = G  [ w ]
 *         [ y ]]   py  [ C2 D21 D22 ]] [ u ]]      [ u ]]
 *
 *        in the form (G, n, mw, pz, gamma).  Solve the appropriate
 *        Riccati equations and return Gc of the H-infinity compensator
 *        defined as
 *
 *         .             n  py
 *      [[ q ]   = n  [[ Ac Bc ] [[ q ]  = Gc [[ q ]
 *       [ u ]]    mu  [ Cc 0  ]] [ y ]]       [ y ]]
 *
 *      A warning message will be reported if
 *
 *  SEE ALSO
 *      dhinf
 */

Func Matrix hinf(G, n, mw, pz, gamma)
	Matrix G;
	Integer n, mw, pz;
	Real gamma;
{
	Matrix A, B1, B2, C1, C2, D11, D12, D21, D22, pc;
	Matrix X, Y, F, L, Ac, Bc, Cc, Gc;
	Matrix IDTD, IDDT, DTD, DDT, TMP;
	Matrix AR, QR, RR;
	Matrix Ab, B2b, C2b, D22b;
	Real tmp;
	Integer i, RG, CG, mu, py;

	RG = Rows(G);
	CG = Cols(G);
	mu = CG - n - mw;
	py = RG - n - pz;

	Gc = Z(n+mu, n+py);

	if (mu <= 0 || py <= 0) {
		error("hinf(): Dimension error.\n");
	}

	A   = G(        1:n,           1:n);
	B1  = G(        1:n,      n+1:n+mw);
	B2  = G(        1:n,     n+mw+1:CG);

	C1  = G(   n+1:n+pz,           1:n);
	D11 = G(   n+1:n+pz,      n+1:n+mw);
	D12 = G(   n+1:n+pz,     n+mw+1:CG);

	C2  = G(  n+pz+1:RG,           1:n);
	D21 = G(  n+pz+1:RG,      n+1:n+mw);
	D22 = G(  n+pz+1:RG,     n+mw+1:CG);

	B1  = B1  / gamma;
	D11 = D11 / gamma;
	D21 = D21 / gamma;

	if ((tmp = maxsing(D11)) >= 1.0) {
		error("hinf(): Gamma is smaller than %g.\n", tmp * gamma);
	}

	if (minsing(D12) <= 0.0) {
		error("hinf(): D12 is not of full rank !\n");
	}

	if (minsing(D21') <= 0.0) {
		error("hinf(): D21 is not of full rank !\n");
	}

	IDTD = (I(Cols(D11)) - D11'*D11)~;
	IDDT = (I(Rows(D11)) - D11*D11')~;
	DTD  = (D12'*IDDT*D12)~;

	TMP  = B2 + B1*IDTD*D11'*D12;
	AR   = A + B1*IDTD*D11'*C1 - TMP*DTD*D12'*IDDT*C1;
	QR   = C1'*IDDT*C1 - C1'*IDDT*D12*DTD*D12'*IDDT*C1;
	RR   = TMP*DTD*TMP' - B1*IDTD*B1';
	X    = Ric(AR, QR, RR);

	pc = eigval(X);
	{tmp,i} = minimum(Re(pc));
	if (tmp <= 0.0) {
		warning("\nhinf(): X may not be positive definite.\n");
		warning("The smallest eigenvalue of X is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}

	TMP  = IDTD*(D11'*C1 + B1'*X);
	Ab   = A + B1*TMP;
	B2b  = B2 + B1*IDTD*D11'*D12;
	C2b  = C2 + D21*TMP;
	D22b = D22 + D21*IDTD*D11'*D12;
	DDT  = (D21*IDTD*D21')~;
	TMP  = D12'*D11*IDTD*(D11'*C1+B1'*X) + D12'*C1 + B2'*X;
	F    = -DTD*TMP;
	RR   = C2b'*DDT*C2b - TMP'*DTD*TMP;
	TMP  = B1*IDTD*D21';
	AR   = (Ab - TMP*DDT*C2b)';
	QR   = B1*IDTD*B1' - TMP*DDT*TMP';
	Y    = Ric(AR, QR, RR);

	pc = eigval(Y);
	{tmp,i} = minimum(Re(pc));
	if (tmp <= 0.0) {
		warning("\nhinf(): Y may not be positive definite.\n");
		warning("The smallest eigenvalue of Y is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}

	pc = eigval(X*Y);
	{tmp,i} = maximum(Re(pc));
	if (tmp >= gamma^2) {
		warning("\nhinf(): eigval(X * Y) >= gamma^2.\n");
		warning("The smalllest eigenvalue of (X * Y) is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}

	L  = -(Y*C2b' + TMP)*DDT;
	Ac = Ab + B2b*F + L*C2b + L*D22b*F;
	Bc = -L;
	Cc = F;

	Gc = [[ Ac,          Bc          ]
	      [ Cc, Z(Rows(Cc), Cols(Bc))]];

	return Gc;
}

Func Matrix Hinf(G, n, mw, pz, gamma)
	Matrix G;
	Integer n, mw, pz;
	Real gamma;
{
	return hinf(G, n, mw, pz, gamma);
}