function [K, X, Z, gamopt, A1, R1, Q1] = coprimeunc(Gs, gammarel)
%
% Finds the controller which optimally robustifies a given
% shaped plant in terms of tolerating maximum coprime uncertainty.
% Inputs:
%   Gs       - System containig the shaped plant.
%   gammarel - Final gamma used is gamma is: gamme = gammarel*gamopt
% Outputs:
%   K        - "Robustifying" controller assuming positive feedback.
%   X        - Solution to the ricatti eqn:
%                             A1'*X + X*A1 
%                               - X*R1*X          + Q1          == 0 
%              (A-B*inv(S)*D'*C)'*X + X*(A-B*inv(S)*D'*C) 
%                               - X*B*inv(S)*B'*X + C'*inv(R)*C == 0
%   Z        - Solution to the ricatti eqn:
%                             A1*X + X*A1' 
%                               - X*Q1*X          + R1          == 0 
%              (A-B*inv(S)*D'*C)*Z + Z*(A-B*inv(S)*D'*C)' 
%                               - Z*C'*inv(R)*C*Z + B*inv(S)*B' == 0
%
%
%

% find normalized coprime factors of the shaped plant

[A,B,C,D] = unpck(Gs);
S = eye(size(D'*D)) + D'*D; R = eye(size(D*D')) + D*D';
Rinv = inv(R); Sinv = inv(S);

A1 = A-B*Sinv*D'*C; Q1 = C'*Rinv*C; R1 = B*Sinv*B';
ham1 = [A1 R1;Q1 -A1'];
[X1,X2,fail,reig_min] = ric_schr(ham1);
if (fail>=1)
   disp('*** WARNING! X-Riccati Equation may be ill-posed ***')
else
   X = X2/X1;
end

A2 = A1'; Q2 = R1; R2 = Q1;
ham2 = [A2 R2; Q2 -A2']; 
[Z1,Z2,fail,reig_min] = ric_schr(ham2);
if (fail>=1)
   disp('*** WARNING! Z-Riccati Equation may be ill-posed ***')
else
   Z = Z2/Z1;
end

XZ = X*Z; gamopt=sqrt(1+max(eig(XZ)));
disp(['Optimal/minimal gamma value: ', num2str(gamopt), '.']);

% Use higher gamma, gamma = gammarel*gamopt;
gam = gammarel*gamopt; gam2 = gam*gam;  gamconst = (1-gam2)*eye(size(XZ));
Lc  = gamconst + XZ;   Li   = inv(Lc'); Fc       = -Sinv*(D'*C+B'*X);
Ac  = A+B*Fc + gam2*Li*Z*C'*(C+D*Fc);
Bc = gam2*Li*Z*C'; Cc = B'*X; Dc = -D';

K = pck(Ac,Bc,Cc,Dc);




