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

%% Setup
% Load system parameters
lbsync = load('lbsync.mat');

% Build model from parameters
tunit = 'seconds';
tunitexp = 0;
sys = build_link_model(lbsync.sim.link.short, 15, tunit);

% Plant output disturbance coloring filter
Fd = zpk(sys.Fd);
Fd.P{1}(end) =  10^tunitexp * -1e1 * 2 * pi; % Change integral behaviour to frequency region of interest
Fd = balreal(Fd);
Fd.D = 0 * Fd.D; % Make proper
Fd.u = 'w'; Fd.y = 'd';

% Synthesize stabilizing reference dynamic output feedback controller for nominal system
%G_nom = connect(sys.G, Fd, sumblk('y = l + d'), sumblk('z = y'), {'w', 'u'}, {'z', 'y'});
% try % We are a bit short on robust control toolbox licenses...
%     [K, Gc, gamma] = h2syn(G_nom, 1, 1);
%     save('reference_link_ctrl.mat', 'K'); % Cache in case there's no license available next time.
% catch e
%     if strcmp(e.identifier, 'MATLAB:license:checkouterror')
%         fprintf('Cannot obtain licenses for h2syn!\n');
%         try
%             load('reference_link_ctrl.mat', 'K');
%             fprintf('Using cached controller...\n');
%         catch e
%             K = tf(0, 1);
%             fprintf('Using zero controller...\n');
%         end
%     else
%         rethrow(e)
%     end
% end

% Simple controller
K = -1 * pid(1e-3); %-1 -> negative feedback
K = chgTimeUnit(K, tunit);
K.y = 'u'; K.u = 'y';


%% Robust Performance Analysis
G = connect(sys.G, Fd, sumblk('y = l + d'), sumblk('q = u'), {'u', 'w'}, {'q', 'y'});
nu = sum(startsWith(G.InputName, 'u'));
nw = sum(startsWith(G.InputName, 'w'));
ny = sum(startsWith(G.OutputName, 'y'));
nq = sum(startsWith(G.OutputName, 'q'));
np = nu;
G = ss(G.A, [G.B(:,1:nu)/2 G.B(:,1:nu)/2 G.B(:,nu+1:end)], G.C, [zeros(nq+ny, np) G.D], 'TimeUnit', G.TimeUnit, 'InputName', {'p', 'u', 'w'}, 'OutputName', {'q', 'y'});

n = order(G);

% Close loop with reference controller and add S delay loop transformation
G_ = G; G_.InputName{1} = 'p_';
G_cl = connect(G_, K, sumblk('p_ = p + q'), {'p', 'w'}, {'q', 'y'});

% Get IQC for delay
% Pi = tf([0 -1; -1 -1]); % Circle, doesn't work for our interconnection
Pi21 = tf([-1.0012/sys.delay^3, 0.0317/sys.delay^2, 12.7106/sys.delay, 0.0003], [1/sys.delay^3, -1.1535/sys.delay^2, -12.7616/sys.delay, -25.6484]); % Pfifer Pi3 custom real rational approximation
Pi = [1e-3 Pi21'; Pi21 -1];
Pi = chgTimeUnit(Pi, tunit);

[Psi, P] = iqc_factorization(Pi);

% Match dimensionality
% TODO: Proove that this is correct
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)), 'TimeUnit', Psi.TimeUnit);
%Psi = balreal(Psi);

% Get Kalman decomposition of Psi with respect to Psi1.
Psi1 = ss(Psi.A, Psi.B(:, 1:nq), Psi.C, Psi.D(:, 1:nq), 'TimeUnit', Psi.TimeUnit);
[~,U] = minreal(Psi1, [], false);
Psi = truncate_coeff(ss(U*Psi.A*U', U*Psi.B, Psi.C*U', Psi.D, 'TimeUnit', Psi.TimeUnit), 1e-10);

% IQC Gridding (no performance)
Gpsi = connect(G_, K, sumblk('p_ = p + q'), {'p'}, {'q', 'p'});
fdi = Gpsi' * Pi * Gpsi;
w = 2*pi*logspace(-2, 6, 2^16) * 10^tunitexp;
resp = freqresp(fdi, w);
eigv = cell2mat(arrayfun(@(i) eig(resp(:,:,i)), 1:size(resp,3), 'uni', 0));
figure; semilogx(w, real(eigv'));

% IQC Gridding (with performance)
Gpsi = connect(G_, K, sumblk('p_ = p + q'), {'p'}, {'q', 'p', 'y'});
fdi = Gpsi' * blkdiag(Pi, eye(ny)) * Gpsi;
w = 2*pi*logspace(-2, 6, 2^16) * 10^tunitexp;
resp = freqresp(fdi, w);
eigv = cell2mat(arrayfun(@(i) eig(resp(:,:,i)), 1:size(resp,3), 'uni', 0));
figure; semilogx(w, real(eigv'));

Gpsi = connect(G_, K, sumblk('p_ = p + q'), {'p', 'w'}, {'q', 'p', 'y'});
Gpsi = blkdiag(Psi, eye(ny)) * Gpsi;

% Model scaling & truncation
Gpsi_scaled = prescale(Gpsi, {1e-1/(2*pi) * 10^tunitexp, 1e6/(2*pi) * 10^tunitexp});
Gpsi_truncated = truncate_coeff(Gpsi_scaled, 1e-11);
% figure; bode(Gpsi, Gpsi_scaled, Gpsi_truncated); % Verify model still has good fit.

Gpsi = Gpsi_truncated;
clear Gpsi_truncated Gpsi_scaled;



A = Gpsi.A; B = Gpsi.B; Bp = B(:, 1:np); Bw = B(:, np+1:end); C = Gpsi.C; Cq = C(1:nq+np,:); D = Gpsi.D; Dq = D(1:nq+np,:); Dp = D(:, 1:np);
k = rank(ctrb(Psi.A, Psi.B(:, 1:nq)));
l = order(Psi) - k;
A1 = Psi.A(1:k, 1:k); A2 = Psi.A(k+1:end,k+1:end); A3 = Psi.A(1:k, k+1:end);
B1 = Psi.B(1:k, 1:nq); B2 = Psi.B(k+1:end, nq+1:end); B3 = Psi.B(1:k, nq+1:end);
C1 = Psi.C(:, 1:k); C2 = Psi.C(:, k+1:end);
D1 = Psi.D(:, 1:nq); D2 = Psi.D(:, nq+1:end);

X = sdpvar(order(Gpsi));
Y = sdpvar(k, k); % Paper says nothing about symmetry of Y?

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

% Constraints
F = [];

%- Margin
%eps = 0;
%eps = sdpvar(); % Objective will be maximizing margin.
eps = 1e-8;

%- KYP
U = [eye(size(A)) zeros(size(Bp));
     A Bp;
     C Dp];
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
% U = [eye(size(A)) zeros(size(B));
%      A B;
%      C D];
% 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 = [F, cnst + eps * eye(size(cnst)) <= 0];

%- Force positivity on X
% F = [F, X - eps * eye(size(X)) >= 0];

%- CL Stability / Lyapunov
U = [eye(size(A1)) zeros(size(B1));
     A1 B1;
     C1 D1];
V = [zeros(size(Y)) Y zeros(size(Y, 1), size(P, 2));
     Y zeros(size(Y)) zeros(size(Y, 1), size(P, 2));
     zeros(size(P, 1), 2*size(Y, 2)) P];

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


%- Coupling
cnst = [ X(1:k, 1:k) - Y, X(1:k, k+l+1:end); X(k+l+1:end, 1:k) X(k+l+1:end, k+l+1:end) ];
F = [F, cnst - eps * eye(size(cnst)) >= 0];

% Objective
obj = trace(Bw' * X * Bw);

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



%% Grid over link length
opt = sdpsettings('solver', 'mosek', 'verbose', 0, 'debug', 0);
dly = logspace(-6, -4, 1e3);
h2 = Inf * dly;
h2pade = h2;
for i = 1:length(dly)
    Pi21 = tf([-1.0012/dly(i)^3, 0.0317/dly(i)^2, 12.7106/dly(i), 0.0003], [1/dly(i)^3, -1.1535/dly(i)^2, -12.7616/dly(i), -25.6484]); % Pfifer Pi3 custom real rational approximation
    Pi = [1e-3 Pi21'; Pi21 -1];
    Pi = chgTimeUnit(Pi, tunit);

    [Psi, P] = iqc_factorization(Pi);
    P = kron(P, sdpvar(np));
    Psi = ss(Psi);

    % Get Kalman decomposition of Psi with respect to Psi1.
    Psi1 = ss(Psi.A, Psi.B(:, 1:nq), Psi.C, Psi.D(:, 1:nq), 'TimeUnit', Psi.TimeUnit);
    [~,U] = minreal(Psi1, [], false);
    Psi = truncate_coeff(ss(U*Psi.A*U', U*Psi.B, Psi.C*U', Psi.D, 'TimeUnit', Psi.TimeUnit), 1e-10);
    
    Gpsi = connect(G_, K, sumblk('p_ = p + q'), {'p', 'w'}, {'q', 'p', 'y'});
    Gpsi = series(Gpsi, blkdiag(Psi, eye(ny)));

    % Model scaling & truncation
    Gpsi_scaled = prescale(Gpsi, {1e-1/(2*pi) * 10^tunitexp, 1e6/(2*pi) * 10^tunitexp});
    Gpsi_truncated = truncate_coeff(Gpsi_scaled, 1e-11);
    % figure; bode(Gpsi, Gpsi_scaled, Gpsi_truncated); % Verify model still has good fit.

    Gpsi = Gpsi_truncated;
    clear Gpsi_truncated Gpsi_scaled;

    A = Gpsi.A; B = Gpsi.B; Bp = B(:, 1:np); Bw = B(:, np+1:end); C = Gpsi.C; Cq = C(1:nq+np,:); D = Gpsi.D; Dq = D(1:nq+np,:); Dp = D(:, 1:np);
    k = rank(ctrb(Psi.A, Psi.B(:, 1:nq)));
    l = order(Psi) - k;
    A1 = Psi.A(1:k, 1:k); A2 = Psi.A(k+1:end,k+1:end); A3 = Psi.A(1:k, k+1:end);
    B1 = Psi.B(1:k, 1:nq); B2 = Psi.B(k+1:end, nq+1:end); B3 = Psi.B(1:k, nq+1:end);
    C1 = Psi.C(:, 1:k); C2 = Psi.C(:, k+1:end);
    D1 = Psi.D(:, 1:nq); D2 = Psi.D(:, nq+1:end);

    X = sdpvar(order(Gpsi));
    Y = sdpvar(k, k); % Paper says nothing about symmetry of Y?

    % Constraints
    F = [];

    %- Margin
    eps = 1e-8;

    %- KYP
    U = [eye(size(A)) zeros(size(Bp));
         A Bp;
         C Dp];
    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)];

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

    %- Force positivity on X
    %F = [F, X - eps * eye(size(X)) >= 0];
    
    %- CL Stability / Lyapunov
    U = [eye(size(A1)) zeros(size(B1));
         A1 B1;
         C1 D1];
    V = [zeros(size(Y)) Y zeros(size(Y, 1), size(P, 2));
         Y zeros(size(Y)) zeros(size(Y, 1), size(P, 2));
         zeros(size(P, 1), 2*size(Y, 2)) P];

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


    %- Coupling
    cnst = [ X(1:k, 1:k) - Y, X(1:k, k+l+1:end); X(k+l+1:end, 1:k) X(k+l+1:end, k+l+1:end) ];
    F = [F, cnst - eps * eye(size(cnst)) >= 0];

    % Objective
    obj = trace(Bw' * X * Bw);

    % Solve!
    sol_diag = optimize(F, obj, opt);
    
    if sol_diag.problem == 0
        h2(i) = sqrt(value(obj));
        if any(eig(value(X)) < -1e-9)
            fprintf('X not positive definite!\n');
        end
    else
        fprintf('Delay = %g, Solver: %s\n', dly(i), sol_diag.info);
    end
    
    % Pade Reference
    Gcl_pade = pade(connect(G, K, tf(1, 1, 'InputName', 'q', 'OutputName', 'p', 'InputDelay', dly(i)), {'w'}, {'y'}));
    h2pade(i) = norm(Gcl_pade);
end

figure; semilogx(dly, h2); hold on; semilogx(dly, h2pade);


%% Robust Synthesis
Psi_p = [eye(ny) zeros(ny, nu+nw)];

Gpsi = ss(G.A, G.B, [G.C(1:nq, :); zeros(np, n); G.C(nq+1:end, :); zeros(nu+nw, n)], [G.D(1:nq, :); eye(np) zeros(np, nu+nw); G.D(nq+1:end, :); zeros(np, nu+nw) eye(np)]);
Gpsi = series(Gpsi, append(Psi, Psi_p));

X = sdpvar(order(Gpsi));

A = Gpsi.A; B = Gpsi.B; C = Gpsi.C; D = Gpsi.D;
Bq = B(:, 1:nq);
Bu = B(:, nq+1:nq+nu);
Bw = B(:, nq+nu+1:end);
Cp = C;


obj = trace(C*X*C');

% Dualized Problem
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), size(X, 2)), zeros(size(P, 1), size(X, 2)), P ];
U = [ -(A * X + Bu * Y)', -Bw', -Bp';
      eye(size(A)), zeros(size(Bw')), zeros(size(Bp'));
      -(Cq * X + Dqu * Y)', -Dqw', -Dqp' ];

F = [ U' * V * U >= 0,
      (A*X + B*Y) + (A*X + B*Y)' + H*H' + eye(size(X)) * eps <= 0];

% Solve!
sol_diag = optimize(F, [], opt);
check(F);
disp(sol_diag.info);

%% Analyse effect of delay size and feedback gain on stability and performance
res = table('Size', [0 4], 'VariableTypes', {'double', 'double', 'logical', 'double'}, 'VariableNames', {'delay', 'feedback gain', 'stable', 'performance'});

fig = figure; hold on;
for dly = logspace(-6, -4, 12)
    for fb_gain = logspace(-1, 5, 15)
        G_dly = G;
        G_dly.OutputDelay = dly;
        G_dly = G / 2 + G_dly / 2;
        
        G_dly_pade = pade(G_dly, 2);
        
        
        G_fb = feedback(G_dly*fb_gain, 1);
        G_fb_pade = feedback(G_dly_pade*fb_gain, 1);
        stb = isstable(G_fb_pade);
        if stb == 1
            perf = norm(G_fb_pade);
        else
            perf = inf;
        end

        res = [res; {dly, fb_gain, stb, perf}];
    end
    
    figure(fig); rlocus(G_dly_pade);
end

%% Pick a delay which happens to robustly stabilize the feedback loop
G_dly = G;
G_dly.OutputDelay = 40e-6;
G_dly = G / 2 + G_dly / 2;

G_dly_pade = pade(G_dly, 2);


%% Check instability regions in root locus with and without delay
for g = [G G_dly_pade]
    fig = figure;
    popt = pzoptions; popt.Grid = 'off'; popt.FreqUnits = 'Hz'; popt.Title.String = ''; popt.XLabel.String = ''; popt.YLabel.String = '';
    h = rlocusplot(g, popt);
    p = getoptions(h);
    box on;

    %xlim([-150e3 50e3]);
    fig.Units = 'inches';
    fig.Position(3) = 5;
    fig.Position(4) = fig.Position(3) / (3/2);
    set(fig.Children(2), 'FontName', 'Times', 'FontSize', 9);
    set(gcf, 'color', 'w');

    axes('Position', [.15 .15 .25 .35]);
    h = rlocusplot(g, popt);
    xlim([-2e3 1e3]);
    ylim([-108.5e3 -97.5e3]);
    set(gca,'xticklabel',[]);
    set(gca,'yticklabel',[]);
    box on;
    res = findall(gcf, 'Type', 'Axes', 'HandleVisibility', 'off');
    delete(res(1));
end


%% Compare step responses of delay-less, real TDS and Padé approximation
figure; hold on; box on;
step(feedback(G,1), 5e-3);
step(feedback(G_dly,1), 5e-3);
step(feedback(G_dly_pade,1), 5e-3);

axes('Position', [.2 .2 .6 .4]);
hold on;
popt = timeoptions; popt.Grid = 'on'; popt.Title.String = ''; popt.XLabel.String = ''; popt.YLabel.String = '';
stepplot(feedback(G,1), 5e-4, popt);
stepplot(feedback(G_dly,1), 5e-4, popt);
stepplot(feedback(G_dly_pade,1), 5e-4, popt);
box on;
res = findall(gcf, 'Type', 'Axes', 'HandleVisibility', 'off');
delete(res(1));

set(gcf, 'color', 'w');


%% Basic stability test with unit feedback
yalmip('clear')
opt = sdpsettings('solver', 'mosek', 'verbose', 1 , 'debug', 1);

CL = feedback(G_dly_pade, 1);

eps = sdpvar;
X = sdpvar(order(CL));

F = [1 >= eps >= 1e-2, ...
     X - eye(size(X)) * eps >= 0, ...
     CL.A*X + X*CL.A' + eye(size(X)) * eps <= 0];

sol_diag = optimize(F, -eps, opt);



%% Synthesize H2-optimal controller
yalmip('clear');
opt = sdpsettings('solver', 'mosek', 'verbose', 1 , 'debug', 1);

g = G + Fd;

A = g.A;
B = [g.B(1:3); zeros(4, 1)];
H = [zeros(3, 1); g.B(4:7)];
C = g.C;

X = sdpvar(order(g));
Y = sdpvar(1, order(g));
eps = 1e-1;

obj = trace(C*X*C');
F = [X - eye(size(X)) * eps >= 0, ...
     A*X + X*A' + Y'*B' + B*Y + H*H' + eye(size(X)) * eps <= 0];

sol_diag = optimize(F, obj, opt);
K = value(Y) / value(X);


%% Delay IQC test
yalmip('clear');
A = G.A;
Bu = G.B;
Cz = G.C;
Dzp = G.C;
Cq = eye(order(G));



%%


% Assume white output disturbance and fixed reference
% Check upper bound from Annika
figure;
w = linspace(-pi, pi, 1e3);
plot(w, sin(w)./w);
hold on; grid on;
plot(w, (1 - 64.6e-3 * w.^2).^2 ./ (1 + 38e-3 * w.^2 + 100e-6 * w.^4 + 850e-6 * w.^6));
plot(w, (1 - (w / pi).^2) ./ (1 + (1/6 - 1/pi.^2) * w.^2 + (2 / pi^4 - 1 / (6 * pi^2)) * w.^4));



%%
try
    autoArrangeFigures;
end