graphsymbols(P)

function [Gl, Gr] = graphsymbols(P)
%graphsymbols Compute the normalized graph symbols of a system.
%   [Gl,Gr] = graphsymbols(P) computes the normalized left and right graph
%   symbols of a system (Vinnicombe, 2000, pp.292-294). These can be used
%   in robust control as building blocks for the analysis of the effects of
%   uncertainty on system behavior.
%
%   This function uses the same sign and ordering conventions as the
%   reference:
%
%      Gl = [-Ml, Nl]
%      Gr = [Nr; Mr]
%
%   Gl and Gr satisfy the following conditions:
%      * Gl and Gr are stable.
%      * Gl is normalized left coprime: Gl * Gl' = I
%      * Gr is normalized right coprime: Gr' * Gr = I
%      * Gl is a left graph symbol: Gl * [P;I] = 0
%      * Gr is a right graph symbol: [-I,P] * Gr = 0
%
%   These conditions are checked by the function and any detected issues
%   are reported as warnings.
%
%   REFERENCE
%   1. Vinnicombe, G. (2000) Uncertainty and Feedback: H-Infinity 
%      Loop-Shaping and the v-Gap Metric. London: Imperial College.


% Check we can check out Control System Toolbox.
licenseAvailable = logical(license('checkout', 'control_toolbox'));
assert(licenseAvailable, 'rho:missingLicense', ...
    ['Missing license: could not check out a license for Control ' ...
    'System Toolbox.']);


% Check the input arguments
narginchk(1, 1);
if isa(P, 'double')
    P = ss(P); % This syntax is acceptable here.
end
pIsOK = goodClass(P) & isproper(P) & ~isempty(P);
assert(pIsOK, 'graphsymbols:badInput', ...
    ['Bad input: P must be a DOUBLE, SS, TF or ZPK object ' ...
    'representing a proper system.']);

P = ss(P); % convert to state-space
P = sminreal(P); % remove invisible modes
P = minreal(P, [], false); % remove pole-zero cancellations

% Apply the algorithm
isDiscrete = ~isequal(P.Ts, 0.0);
if isDiscrete
    [Gl,Gr] = dtgraphsymbols(P);
else
    [Gl,Gr] = ctgraphsymbols(P);
end

end



%%% Algorithm for continuous-time systems

function [Gl,Gr] = ctgraphsymbols(P)
%ctgraphsymbols Left and right graph symbol of a continuous-time system.
%   [Gl,Gr] = ctgraphsymbols(P) returns the normalized left and right
%   coprime graph symbols of the continuous-time dynamical system P.  It
%   does this by solving appropriate Riccati equations.

% Define a tolerance used to check the consistency of results.
tol = 1e-4;

assert(isequal(P.Ts, 0.0));
P = prescale(P);

% Extract the state-space matrices from P
[A,B,C,D,Ts] = ssdata(P);

% Form derived matrices R and S
[ny,nu] = size(P);
R = eye(ny) + D * D.';
S = eye(nu) + D.' * D;

% Solve the control/filtering AREs and form generalized gains
[X, Z] = solveRiccatiEquations(A, B, C, D, R, S);
[F, H] = formGeneralizedGains(B, C, D, R, S, X, Z);


% Form R^(-0.5) - this term is needed several times
sqrtRInv = real(inv(R) ^ 0.5);
sqrtSInv = real(inv(S) ^ 0.5);

% Form the normalized right graph symbol.
Ar = A + B * F;
Br = B * sqrtSInv;
Cr = [C + D*F; F];
Dr = [D * sqrtSInv; sqrtSInv];

Gr = ss(Ar, Br, Cr, Dr, Ts);
Gr = prescale(Gr);


% Form the normalized left graph symbol.
Al = A + H * C;
Bl = [-H, B + H * D];
Cl = sqrtRInv * C;
Dl = [-sqrtRInv, sqrtRInv*D];

Gl = ss(Al, Bl, Cl, Dl, Ts);
Gl = prescale(Gl);

% Check the results
checkResults(P, Gl, Gr, tol);

end


function [X, Z] = solveRiccatiEquations(A, B, C, D, R, S)
%solveRiccatiEquations Solve the Generalized Control and Filtering AREs.

% Set up and solve the Generalized Control ARE.
AX = (A - B * (S \ (D.'*C))); 
BX = B * (S \ B.'); 
CX = C.' * (R \ C);

X = are(AX, BX, CX);
X = real(X);

% Set up and solve the Generalized Filtering ARE.
AZ = AX.';
BZ = CX;
CZ = BX';
Z = are(AZ, BZ, CZ);
Z = real(Z);

end


function [F,H] = formGeneralizedGains(B, C, D, R, S, X, Z)
%formGeneralizedGains Form the generalized control and filtering gains

%F = -inv(S) * (D.'*C + B.'*X);
F = -S \ (D.'*C + B.'*X);

% H = -(B*D.' + Z*C.') * inv(R);
Ht = (-R.' \ (B*D.' + Z*C.').');
H = Ht.';

end



%%% Algorithm for discrete-time systems

function [Gld,Grd] = dtgraphsymbols(Pd)
%dtgraphsymbols Left and right graph symbol of a discrete-time system.
%   [Gld,Grd] = ctgraphsymbols(Pd) returns the normalized left and right
%   coprime graph symbols of the discrete-time dynamical system Pd.  It
%   does this my mapping the discrete domain to the Laplace domain, solving
%   for a continuous problem, and mapping back.

Ts = Pd.Ts;
assert(~isequal(Ts, 0));

Pd = prescale(Pd);
P = mapToContinuous(Pd);

[Gl,Gr] = ctgraphsymbols(P);

Gld = mapToDiscrete(Gl, Ts);
Grd = mapToDiscrete(Gr, Ts);

Gld = prescale(Gld);
Grd = prescale(Grd);

end



%%% Result checking
% The following functions define checks on the results so we can alert the
% user to any potential numerical problems.

function checkResults(P, Gl, Gr, tol)
%checkResults Check the results are consistent with the inputs.

% Define the checks: format is {check condition, warning message}.
checkDefinitions = {   
    @()myisstable(Gl), 'Gl appears to be unstable.'
    @()myisstable(Gr), 'Gr appears to be unstable.'
    @()isNormLeftCoprime(Gl, tol), 'Gl * Gl'' ~= I.'
    @()isNormRightCoprime(Gr, tol), 'Gr'' * Gr ~= I.'
    @()isLeftGraphSymbol(P, Gl, tol), 'Gl * [P;I] ~= 0.'
    @()isRightGraphSymbol(P, Gr, tol), '[-I,P] * Gr = 0'
};

% Run the checks.
executeFcn = @(chkFcn)chkFcn();
failedCheckIdx = ~cellfun(executeFcn, checkDefinitions(:, 1));
failedChecks = checkDefinitions(failedCheckIdx, 2);

% Report any check failures.
if ~isempty(failedChecks)    
    warnMsg = ['Possible numerical problems detected. ' ...
        'Use results with caution.' ...
        sprintf('\n  * %s', failedChecks{:})];
    warning('graphsymbols:possibleNumericalProblemsDetected', ...
        warnMsg);
end

end


function success = isNormLeftCoprime(Gl, tol)
ny = size(Gl, 1);
Gerr = Gl * Gl' - eye(ny);
Gerr = minreal(Gerr, [], false);
err = linfnorm(Gerr, 0.5*tol);
success = err <= tol;
end


function success = isNormRightCoprime(Gr, tol)
nu = size(Gr, 2);
Gerr = Gr' * Gr - eye(nu);
Gerr = minreal(Gerr, [], false);
err = linfnorm(Gerr, 0.5*tol);
success = err <= tol;
end


function success = isLeftGraphSymbol(P, Gl, tol)
%isLeftGraphSymbol
%   Note that P = inv(Ml) * Nl.
%   Therefore
%   [-Ml Nl] * [P;I] = -Ml*inv(Ml)*Nl  +   Nl 
%                    =         -Nl      +   Nl 
%                    =          0

nu = size(P, 2);
Gerr = Gl * [P; eye(nu)];
Gerr = minreal(Gerr, [], false);
success = approxZeroTest(Gerr, tol);
end



function success = isRightGraphSymbol(P, Gr, tol)
%isRightGraphSymbol
%   Note that P = Nr*inv(Mr)
%   Therefore 
%   [-I,P] * [Nr;Mr] =  -Nr  +  Nr * inv(Mr) * Mr
%                    =  -Nr  +  Nr
%                    =   0

ny = size(P, 1);
Gerr = [-eye(ny), P] * Gr;
Gerr = minreal(Gerr, [], false);
success = approxZeroTest(Gerr, tol);
end



%%% Utility functions
% The functions that follow are low-level utility functions. They exist
% primarily to abstract detail from the functions that call them.


function success = approxZeroTest(Pdelta, tol)
%approxZeroTest Approximate test that a system is zero.

% For stable systems, this check is excellent.
Pdelta = minreal(Pdelta, [], false);
normOK = (hinfnorm(Pdelta, 0.1*tol) < 1.1 * tol);

% For unstable systems, this check is reasonable.
[~,B,C,D] = ssdata(Pdelta);
DCB = [D, C * B];
dcbOK = (max(abs(DCB(:))) < tol);

success = normOK | (dcbOK & ~isstable(Pdelta));
end


function success = myisstable(G)
if ~isct(G)
    G = mapToContinuous(G);   
end
success = isstable(G);
end


function g = hinfnorm(G, tol)
%hinform H-infinity norm of a dynamic system.
%   g = hinform(G, tol) returns the H-infinity norm of a dynamic system.
%   The system is expressed in continuous-time - it's numerically easier to
%   test things like stability against the S-plane than the Z-plane.

if ~isct(G)
    G = mapToContinuous(G);
end
if isstable(G)
    g = norm(G, inf, tol);
else
    g = inf;
end
end


function g = linfnorm(G, tol)
%hinform L-infinity norm of a dynamic system.
%   g = hinform(G, tol) returns the L-infinity norm of a dynamic system.
%   The system is expressed in continuous-time - it's numerically easier to
%   test things like stability against the S-plane than the Z-plane.

if ~isct(G)
    G = mapToContinuous(G);
end
g = norm(G, inf, tol);
end


function Gc = mapToContinuous(G)
%mapToContinuous Map any dynamic system to the continuous domain.
assert(isdt(G))
G.Ts = 1;
Gc = d2c(G, 'tustin');
end


function Gd = mapToDiscrete(G, Ts)
%mapToDiscrete Map any dynamic system to the discrete domain.
assert(Ts ~= 0.0);
Gd = c2d(G, 1, 'tustin');
Gd.Ts = Ts;
end


function success = goodClass(P)
%goodClass True for SS, TF and ZPK objects.
classP = class(P);
success = ismember(classP, {'ss', 'tf', 'zpk'});
end