clear;

%% Config
opt = systuneOptions(RandomStart=100, MaxIter=1e3, SoftTol=1e-5, UseParallel=false, Display='final');


%% Plant
delta = ureal('delta', 0);
G = tf(1, [1, (2+delta), 1]);
G.InputName = 'w'; G.OutputName='z';
Gworst = tf(usubs(G, delta=-1));

[M, Delta, BlkStruct] = lftdata(G, {'delta'});

pidx = strcmp(M.InputName, ''); np = sum(pidx);
qidx = strcmp(M.OutputName, ''); nq = sum(qidx);
M.InputName(pidx) = compose('p(%d)', 1:np);
M.OutputName(qidx) = compose('q(%d)', 1:nq);

widx = startsWith(M.InputName, 'w'); nw = sum(widx);
zidx = startsWith(M.OutputName, 'z'); nz = sum(zidx);


%% Goals
sigma_max_goal = TuningGoal.Gain('v', 'c', 1);
sigma_max_goal.Focus = [0, 1e2];
sigma_max_goal.Stabilize = false;
h2_goal = HermIntegral('k', 'l', 1);
pos_real_goal = TuningGoal.Passivity('k', 'l');


%% Direct Optimization

NPsiY = 1;
data = [];
for N = 1:3
    %- Robust Stability: Constant uncertain parameter
    D = tunableZPK("D", N, nq, nq);
    W = tunableZPK("W", N, nq, nq);

    % Robust performance: H2
    PsiY = tunableZPK("", NPsiY, nw, nw);
    for i = 1:NPsiY, PsiY.Blocks.("a" + i).Minimum = 0; end
    for i = 1:NPsiY, PsiY.Blocks.("b" + i).Minimum = 0; end % doesn't matter if positive or negative
    for i = 1:NPsiY, PsiY.Blocks.("X" + i).Minimum = 0; end
    PsiY.InputName = 'k'; PsiY.OutputName = 'l';

    % Augmented multiplier
    Sigma = tf();
    Psi = tf();
    phi0 = tf();
    Sigma = append(Sigma, D);
    Psi = append(Psi, -(D' * D) / 2);
    phi0 = append(phi0, W' - W);
    Sigma = append(Sigma, eye(nz));
    phi0 = append(phi0, zeros(nw, nz));
    Psi = append(Psi, -PsiY);

    % Construct test system
    ns = size(Sigma, 1);
    sys = sect(-[phi0 * M + Psi, zeros(np+nw, ns); ...
                 Sigma * M, -eye(ns) / 2]);
    sys.InputName = 'v'; sys.OutputName = 'c';
    
    % optimize
    [sol, fsoft, ghard, info] = systune(append(sys, PsiY), h2_goal, [sigma_max_goal, pos_real_goal], opt);
    
    softhard = [vertcat(info.fSoft), max(vertcat(info.gHard), [], 2)];
    [softhardsorted, softhardsortedidx] = sortrows(softhard);
    
    data = [data, (sqrt(2*softhardsorted(2:end,1)) - 1) * 100]; %#ok<AGROW> % Relative robust h2-perf bound gap (sqrt(gamma^2) - sqrt(1/2)) / sqrt(1/2)
end

data = array2table(data);
data.Properties.VariableNames = compose("perf_gap_perc_%d", 1:N);
data.Properties.VariableUnits = repmat("%", 1, N);
writetable(data, "ns_ex_ana.dat", Delimiter='\t');

D_ = ss(replaceBlock(D, sol.Blocks));
W_ = ss(replaceBlock(W, sol.Blocks));
PsiY_ = tf(replaceBlock(PsiY, sol.Blocks));

%% Fixed performance multiplier
N=2;
NPsiY = 1;

%- Robust Stability: Constant uncertain parameter
D = tunableZPK("D", N, nq, nq);
W = tunableZPK("W", N, nq, nq);

% Robust performance: H2
PsiY = tunableZPK("", NPsiY, nw, nw);
for i = 1:NPsiY, PsiY.Blocks.("a" + i).Minimum = 0; end
for i = 1:NPsiY, PsiY.Blocks.("b" + i).Minimum = 0; end % doesn't matter if positive or negative
for i = 1:NPsiY, PsiY.Blocks.("X" + i).Minimum = 0; end
PsiY.InputName = 'k'; PsiY.OutputName = 'l';

% Augmented multiplier
Sigma = tf();
Psi = tf();
phi0 = tf();
Sigma = append(Sigma, D);
Psi = append(Psi, -(D' * D) / 2);
phi0 = append(phi0, W' - W);
Sigma = append(Sigma, eye(nz));
phi0 = append(phi0, zeros(nw, nz));
Psi = append(Psi, -PsiY);

% Construct test system
ns = size(Sigma, 1);
sys = sect(-[phi0 * M + Psi, zeros(np+nw, ns); ...
             Sigma * M, -eye(ns) / 2]);
sys.InputName = 'v'; sys.OutputName = 'c';

sys.Blocks.a1.Value = 1/2;
sys.Blocks.a1.Free = 0;
sys.Blocks.b1.Value = sqrt(3/4);
sys.Blocks.b1.Free = 0;
sys.Blocks.X1.Free = 0;
sys = replaceBlock(sys, "Y1", sys.Blocks.X1 / sqrt(16/3)); % sqrt(16/3) = 2/sqrt(3/4)

gap = logspace(-2, 0, 10);
sigmamax = [];
for eps = gap
    sys.Blocks.X1.Value = 0.5 + eps;
    [~, ~, ~, i] = systune(sys, sigma_max_goal, [], opt);
    sigmamax = [sigmamax, sort(vertcat(i.fSoft))]; %#ok<AGROW>
end; clear eps i;

data = table(100*(sqrt(2*(0.5 + gap')) - 1), sum(sigmamax < 1 + 1e-5)', VariableNames=["perf_gap_perc", "prob"]);
writetable(data, "ns_ex_ana_prob.dat", Delimiter='\t');

semilogx(data.perf_gap_perc, data.prob, '-o');

%% Statistics
figure(); hold('on'); grid('on');
yyaxis('left');
scatter(1:length(info), softhardsorted(:,1), 'filled');
set(gca, YScale='log'); ylabel('Soft Objective');
if size(softhard, 2) > 1
    yyaxis('right');
    scatter(1:length(info), softhardsorted(:,2), 'filled');
    ylim([min(min(softhard(:,2)), 1), min(max(softhard(:,2)), 1.01)+eps]);
    set(gca, YScale='log'); ylabel('Hard Objective');
    yyaxis('left');
end
title('Achieved objective values over random starts');
xlabel('Run Number');

%% Validate performance multiplier
% Bound visualization
figure(); bode(Gworst'*Gworst, PsiY_ + PsiY_');
figure(); bode((PsiY_ + PsiY_') - Gworst'*Gworst);

% FDI gridding
mult = append(eye(nz), -PsiY_-PsiY_');
test = M(np+1:end, nq+1:end);
fdisys = [ss(test); eye(nw)]' * mult * [ss(test); eye(nw)];
f = logspace(-2, 6, 1e4);
fdiresp = freqresp(fdisys, f);
assert(sum(diag(sum(abs(imag(fdiresp)), 3))) < 1e-6);
fdiresp = make_diag_real(fdiresp);
res = cell2mat(arrayfun(@(page) eig(fdiresp(:,:,page)), 1:size(fdiresp,3), 'UniformOutput', false));
res2 = cell2mat(arrayfun(@(page) out2(@() chol(-fdiresp(:,:,page))), 1:size(fdiresp,3), 'UniformOutput', false));
figure();
semilogx(f, sort(real(res))'); yyaxis('right');
semilogx(f, res2 > 0); ylim([-0.5, 1.5]); yyaxis('left');


%% Validate robustness multiplier
% FDI gridding
mult = [D_'*D_, W';
        W, -D_'*D_];
test = M(1:np, 1:nq);
fdisys = [ss(test); eye(np)]' * mult * [ss(test); eye(np)];
f = logspace(-2, 6, 1e4);
fdiresp = freqresp(fdisys, f);
assert(sum(diag(sum(abs(imag(fdiresp)), 3))) < 1e-6);
fdiresp = make_diag_real(fdiresp);
res = cell2mat(arrayfun(@(page) eig(fdiresp(:,:,page)), 1:size(fdiresp,3), 'UniformOutput', false));
res2 = cell2mat(arrayfun(@(page) out2(@() chol(-fdiresp(:,:,page))), 1:size(fdiresp,3), 'UniformOutput', false));
figure();
semilogx(f, sort(real(res))'); yyaxis('right');
semilogx(f, res2 > 0); ylim([-0.5, 1.5]); yyaxis('left');


%% Validate both
% FDI gridding
mult = [blkdiag(D_'*D_, eye(nz)), blkdiag(W_ - W_', zeros(nz, nw));
        blkdiag(W_' - W_, zeros(nw, nz)), blkdiag(-D_'*D_, -PsiY_-PsiY_')];
fdisys = [ss(M); eye(nw+np)]' * mult * [ss(M); eye(nw+np)];
f = logspace(-2, 6, 1e4);
fdiresp = freqresp(fdisys, f);
assert(sum(diag(sum(abs(imag(fdiresp)), 3))) < 1e-6);
fdiresp = make_diag_real(fdiresp);
res = cell2mat(arrayfun(@(page) eig(fdiresp(:,:,page)), 1:size(fdiresp,3), 'UniformOutput', false));
res2 = cell2mat(arrayfun(@(page) out2(@() chol(-fdiresp(:,:,page))), 1:size(fdiresp,3), 'UniformOutput', false));
figure();
semilogx(f, sort(real(res))'); yyaxis('right');
semilogx(f, res2 > 0); ylim([-0.5, 1.5]); yyaxis('left');


%% Cleanup
try %#ok<TRYNC>
    autoArrangeFigures();
    % bring_figures_to_front();
end