function [ak,bk,ck,dk,Xinf,Yinf,rho]=hinfgd(Ap,Bp,Cp,Dp,m1,m2,p1,p2,gamma);
%  [ak,bk,ck,dk,Xinf,Yinf,rho]=hinfgd(Ap,Bp,Cp,Dp,m1,m2,p1,p2,gamma)
%
%  [ak,bk,ck,dk]=hinfgd(Ap,Bp,Cp,Dp,m1,m2,p1,p2,gamma)
%
% This function implements the Glover-Doyle algorithm for H-infinity
% synthesis, following section 6.7 of Maciejowski: Multivariable Feedback
% Design (Addison-Wesley 1989).
%
% This version assumes that (6.181) holds, and that m2 ~= p1. 
%
% It uses function `mylqr', which is the Control System Toolbox's
% function lqr with checking for definiteness supressed.

% Copyright (C) 1992, J.M.Maciejowski.
% J.M.Maciejowski, Cambridge University Engineering Dept, 11 Feb 1992.

% Check arguments:
abcdchk(Ap,Bp,Cp,Dp);
[nout,nin] = size(Dp);
if m1+m2 ~= nin,
  error('m1 and m2 do not agree with #inputs of P')
end
if p1+p2 ~= nout,
  error('p1 and p2 do not agree with #outputs of P')
end

gammasq = gamma^2;
D1star = Dp(1:p1,:);  Dstar1 = Dp(:,1:m1);                 % (6.186)
R = D1star'*D1star - [gammasq*eye(m1), zeros(m1,m2) ;
                      zeros(m2,m1),    zeros(m2,m2)  ];    % (6.187)
Rtilde = Dstar1*Dstar1' - [gammasq*eye(p1), zeros(p1,p2) ;
                           zeros(p2,p1),    zeros(p2,p2)  ]; %(6.187)

Bp1 = Bp(:,1:m1);         Cp1 = Cp(1:p1,:);
Bp2 = Bp(:,m1+1:m1+m2);   Cp2 = Cp(p1+1:p1+p2,:);

[dum,Xinf] = mylqr(Ap-(Bp/R)*D1star'*Cp1,Bp,...
             Cp1'*(eye(p1)-D1star*(R\D1star'))*Cp1,R);    % (6.189)
[dum,Yinf] = mylqr((Ap-Bp1*Dstar1'*(Rtilde\Cp))',Cp',...
             Bp1*(eye(m1)-Dstar1'*(Rtilde\Dstar1))*Bp1',Rtilde); %(6.190)

F = -R\(D1star'*Cp1+Bp'*Xinf);                            % (6.191)
H = -(Bp1*Dstar1'+Yinf*Cp')/Rtilde;                       % (6.192)

rho = max(eig(Xinf*Yinf));                                % (6.196)
disp(['Gamma squared is ',num2str(gammasq),'.   Rho is ',num2str(rho)'])
if rho >= gammasq,
  disp('Gamma is too small. Try a larger value.')
end

n = max(size(Ap));        % State dimension
Zinv = eye(n)-Yinf*Xinf/gammasq;                          % (6.207)
                                %(NB Book has misprint here)

Dp11  = Dp(1:p1,1:m1);
D1111 = Dp11(1:p1-m2,1:m1-p2);                            % (6.182)
D1112 = Dp11(1:p1-m2,m1-p2+1:m1);
D1121 = Dp11(p1-m2+1:p1,1:m1-p2);
D1122 = Dp11(p1-m2+1:p1,m1-p2+1:m1);

if isempty(D1111) | isempty(D1121),  % This happens if m1=p2.
                                     % Should also check for m2=p1.
  D11hat = -D1122;
  D12hat = eye(m2);
  D21hat = chol(eye(p2)-D1112'*D1112/gammasq);
else
  gamID = (gammasq*eye(p1-m2)-D1111*D1111')\D1112; % For (6.199) and (6.201)
  D11hat = -D1121*D1111'*gamID-D1122;                     % (6.199)
  D12hat = chol(eye(m2)-D1121*((gammasq*eye(m1-p2)-D1111*D1111')\D1121'));
                                                          % (6.200)
  D21hat = chol(eye(p2)-D1112'*gamID);                    % (6.201)
end

B2hat = (Bp2+H(:,p1-m2+1:p1))*D12hat;                     % (6.202)
C2hat = -D21hat*(Cp2+F(m1-p2+1:m1,:))/Zinv;               % (6.203)
B1hat = -H(:,p1+1:p1+p2)+B2hat*(D12hat\D11hat);           % (6.204)
C1hat = F(m1+1:m1+m2,:)/Zinv+(D11hat/D21hat)*C2hat;       % (6.205)
Ahat  = Ap+H*Cp+B2hat*(D12hat\C1hat);                     % (6.106)

% Central controller Ktilde has realization: (Ahat,B1hat,C1hat,D11hat)
ak = Ahat;   bk = B1hat;
ck = C1hat;  dk = D11hat;

% Now have to scale it to get K = Su\Ktilde*Sy  (p.301).
