close all;
yalmip('clear');
clear;

np = 1;
nq = 1;
nu = 1;
ny = 1;

% Unstable plant
G = tf([1 1], [1 2 3]) * tf(1, [1 -0.1]);
G.InputName = 'u'; G.OutputName = 'l';

% Output disturbance weighting (needed so we don't get unwanted feedthroughs)
% Fd = tf(1, [1 1], 'InputName', 'w', 'OutputName', 'd'); % Only needed for synthesis


% Nominal Stabilizing Controller (negative feedback)
K = ss(tf(2, 1, 'InputName', 'e', 'OutputName', 'u'));

% Nominal Closed Loop Plant
G_nom_cl = connect(G, K, sumblk('y = l + d'), sumblk('e = r - y'), {'r'}, {'y'});

% Real Plant, input arrives 50% delayed.
G = ss(G);
G = ss(G.A, [G.B G.B] / 2, G.C, G.D, 'InputName', {'p', 'u'}, 'OutputName', {'l'});
G = connect(G, K, sumblk('e = -l'), sumblk('q = u'), {'p'}, {'q'});
h_max = 0.1; % delay

% Robust Analysis
%Pi = [0.01 -1; -1 -1]; % Circle
%Pi = tf([-2.19/h_max^2 9.02/h_max 0.089], [1/h_max^2 -5.64/h_max -17]) / 2; % Pfifer Pi3, Real Rational
%Pi = [0, Pi'; Pi, -1];
load('delay_multiplier.mat', 'Pi'); % Pfifer Pi3, Custom Real Rational
[Psi, P] = iqc_factorization(Pi);

P = kron(P, sdpvar(np));
Psi = ss(Psi);
Psi = ss(kron(Psi.A, eye(np)), kron(Psi.B, eye(np)), kron(Psi.C, eye(np)), kron(Psi.D, eye(np)));

% Transform to S
G_ = G; G_.InputName{1} = 'p_';
G_ = connect(G_, sumblk('p_ = p + q'), {'p'}, {'q', 'p'}); % This creates [G I]' of the FDI, e.g. Veenman 2016, (8)

% IQC Gridding
fdi = G_' * Pi * G_;
%fdi = Gpsi_' * Pi * Gpsi_;
w = 2*pi*logspace(-2, 6, 2^16);
resp = freqresp(fdi, w);
eigv = cell2mat(arrayfun(@(i) eig(resp(:,:,i)), 1:size(resp,3), 'uni', 0));
figure; semilogx(w, real(eigv'));

Gpsi = Psi * G_;



A = Gpsi.A; B = Gpsi.B; C = Gpsi.C; D = Gpsi.D;

X = sdpvar(order(Gpsi));
eps = 1e-4;

opt = sdpsettings('solver', 'mosek', 'verbose', 1 , 'debug', 1);

U = [eye(size(A)) zeros(size(B));
     A B;
     C D];

% with performance
% V = [zeros(size(X)) X zeros(size(X, 1), size(P, 2)+ny);
%      X zeros(size(X)) zeros(size(X, 1), size(P, 2)+ny);
%      zeros(size(P, 1), 2*size(X, 2)) P zeros(size(P, 1), ny);
%      zeros(ny, 2*size(X, 2) + size(P, 2)) eye(ny)];

% no performance
V = [zeros(size(X)) X zeros(size(X, 1), size(P, 2));
     X zeros(size(X)) zeros(size(X, 1), size(P, 2));
     zeros(size(P, 1), 2*size(X, 2)) P];

cnst = U' * V * U;
F = [cnst + eps * eye(size(cnst)) <= 0,
     X - eps * eye(size(X)) >= 0];

sol_diag = optimize(F, -eps, opt);
sol_diag = optimize(F, [], opt);
check(F);
disp(sol_diag.info);