/* -*- MaTX -*-
 *
 *  NAME
 *      dhinf() - Solve H_infinity Problem (Discrete-time case)
 *
 *  SYNOPSIS
 *      Gc = dhinf(G, n, mw, pz, gamma)
 *         Matrix G;
 *         Integer n, mw, pz;
 *         Real gamma;
 *
 *      Gc = dhinf(G, n, mw, pz, gamma, aug_flag)
 *         Matrix G;
 *         Integer n, mw, pz;
 *         Real gamma;
 *         Integer aug_flag;
 *
 *  DESCRIPTION
 *      dhinf(G, n, mw, pz, gamma)
 *
 *      Given the generalized plant
 *
 *                              n  mw  mu
 *        [[ x(k+1) ]     n  [[ A  B1  B2  ] [[ x(k) ]      [[ x ]
 *         [ z(k)   ]  = pz   [ C1 D11 D12 ]  [ w(k) ]  = G  [ w ]
 *         [ y(k)   ]]   py   [ C2 D21 D22 ]] [ u(k) ]]      [ 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(k+1) ]   = n  [[ Ac Bc ] [[ q(k) ]  = Gc [[ q ]
 *         [ u(k)   ]]    mu  [ Cc 0  ]] [ y(k) ]]       [ y ]]
 *     
 *         by W.Manop
 *
 *  SEE ALSO
 *      hinf and ric
 */

Func Matrix dhinf(G, n, mw, pz, gamma, aug_flag, ...)
	Matrix G;
	Integer n, mw, pz;
	Real gamma;
	Integer aug_flag;
{
	Matrix A, B1, B2, C1, C2, D11, D12, D21, D22;
	Matrix X, Y, F, L, Ac, Bc, Cc, Gc;
	Matrix IDTD, IDDT, DTD, DDT, TMP;
	Matrix AR, QR, RR, HL, HR;
	Matrix B, D, TMP2, B3;
	Matrix S, T, U, V;
	Matrix Ab, B2b, C2b, D22b, pc;
	Integer i, RG, CG, mu, py;
	Real tmp, eps1, eps2;
	Index idx;

	error(nargchk(5, 6, nargs, "dhinf"));
	
	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("dhinf(): 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);

	if (nargs == 6) {
		eps1 = EPS;
		eps2 = EPS;
		read eps1, eps2;
		B1 = [B1,eps2*I(n),Z(n,py)];
		C1 = [[C1][eps1*I(n)][Z(mu,n)]];
		D11 = [[  D11   , Z(pz,n), Z(pz,py)]
			   [Z(n,mw) , Z(n,n) , Z(n,py)]
			   [Z(mu,mw), Z(mu,n), Z(mu,py)]];
		D12 = [[D12][Z(n,mu)][eps1*I(mu)]];
		D21 = [D21,Z(py,n),eps2*I(py)];
		mw = n+mw+py;
		pz = n+pz+mu;
	}

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

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

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

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

	IDTD = (I(mw) - D11'*D11)~;
	IDDT = (I(pz) - 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';

	HR = [[I(n), -RR]
	      [Z(n), AR']];
	HL = [[AR,  Z(n)]
	      [-QR, I(n)]];
	
	{S,T} = eig(HL,HR);
	{S,idx} = sort(diag2vec(S));
	idx = fliplr(idx);
	S = flipud(S);
	T = T(:,idx);
	
	if (version() == 4) {
		U = T(0, 1, A);
		V = T(1, 1, A);
	} else {
		U = T(1, 2, A);
		V = T(2, 2, A);
	}
	X = Re(V * U~);

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

	B = [B1 B2];
	D = [D11 D12];
	TMP = [[I(mw) Z(mw,mu)][Z(mu,mw) Z(mu,mu)]];
	F = -[Z(mu,mw) I(mu)]*(D'*D - TMP)~*(B'*X*A + D'*C1);

	pc = eigval(A + B2*F);
	{tmp,i} = maximum(abs(Array(pc)));
	if (tmp >= 1.0) {
		warning("\ndhinf(): State feedback part is unstable.\n");
		warning("The largest eigenvalue of A + B2 F is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}

	TMP  = (I(mw)-D11'*D11-B1'*X*B1)~;
	B3   = B1'*X*B2 + D11'*D12;
	Ab   = A + B1*TMP*(D11'*C1 + B1'*X*A);
	B2b  = B2 + B1*TMP*B3;
	C2b  = C2 + D21*TMP*(D11'*C1 + B1'*X*A);
	D22b = D22 + D21*TMP*B3;
	TMP2 = (D12'*D12 + B2'*X*B2 + B3'*TMP*B3);

	DTD  = (D21*TMP*D21')~;
	AR   = (Ab - B1*TMP*D21'*DTD*C2b)';
	QR   = B1*TMP*B1' - B1*TMP*D21'*DTD*D21*TMP*B1';
	RR   = C2b'*DTD*C2b - F'*TMP2*F;

	HR = [[I(n), -RR]
	      [Z(n), AR']];
	HL = [[AR,  Z(n)]
	      [-QR, I(n)]];
	
	{S,T} = eig(HL,HR);
	{S,idx} = sort(diag2vec(S));
	idx = fliplr(idx);
	S = flipud(S);
	T = T(:,idx);
	
	if (version() == 4) {
		U = T(0, 1, A);
		V = T(1, 1, A);
	} else {
		U = T(1, 2, A);
		V = T(2, 2, A);
	}
	Y = Re(V * U~);

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

	TMP  = (I(n) - Y*F'*TMP2*F)~*Y*C2b';
	TMP2 = (I(mw)-D11'*D11-B1'*X*B1)~;
	L = - (B1*TMP2*D21' + A*TMP)*(D21*TMP2*D21' + C2b*TMP)~;

	pc = eigval(A + L*C2b);
	{tmp,i} = maximum(abs(Array(pc)));
	if (tmp >= 1.0) {
		warning("\ndhinf(): State observer part is unstable.\n");
		warning("The largest eigenvalue of A + L C2b is (%g,%g)\n",
				Re(pc(i)), Im(pc(i)));
	}

	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 DHinf(G, n, mw, pz, gamma, aug_flag, ...)
	Matrix G;
	Integer n, mw, pz;
	Real gamma;
	Integer aug_flag;
{
	error(nargchk(5, 6, nargs, "DHinf"));
	
	if (nargs == 5) {
		return dhinf(G, n, mw, pz, gamma);
	} else {
		return dhinf(G, n, mw, pz, gamma, aug_flag);
	}
}