clear; close all;  % not needed, except for debugging

%%% CONFIGURATION %%% --------------------------------------------------------------------------------------------------
%% Set user-defined config parameters
model      = "roundtrip";          % either of "ppl", "roundtrip"
time_unit  = "milliseconds";
scaling    = 15;             % model unit scale, 15 = femtosecond, etc.
pade_order = 1;
bandlimit  = inf;
min_decay  = 1;

% validate & process
model = lower(model);

% Other stuff used throughout the script
h2_goal = TuningGoal.Variance("w", "z", 1);


%%% Build model %%% ----------------------------------------------------------------------------------------------------
%% Load model parameter set
datapth = "../../../../../Data";

try
    load("models/lbsync_final.mat");
catch
    lbsync = {load("models/lbsync.mat")};
    lbsync = [lbsync, load(fullfile(datapth, "2024-07-01 XFEL Characterization", "data/lbsync.mat"))];
    lbsync = [lbsync, load(fullfile(datapth, "2024-10-10 XFEL Characterization", "data/lbsync.mat"))];
    lbsync = [lbsync, load(fullfile(datapth, "2024-12-11 XFEL Characterization", "data/lbsync.mat"))];
    lbsync = [lbsync, load(fullfile(datapth, "2024-12-12 XFEL Characterization", "data/lbsync.mat"))];
    lbsync = [lbsync, load("models/lbsync_single_link_pz_res.mat")];
    lbsync = merge_structs(lbsync{:});

    input_scaling = 1e-3;
    lbsync.xfel.laser.mlo1.input_scaling     = input_scaling;
    lbsync.xfel.laser.slo1.input_scaling     = input_scaling;
    lbsync.xfel.laser.sase3sxp.input_scaling = input_scaling;
    lbsync.xfel.link.slo1.input_scaling      = input_scaling;
    lbsync.xfel.link.relonk.input_scaling    = input_scaling;
    lbsync.xfel.link.sase3sxp.input_scaling  = input_scaling;
    clear input_scaling;
end

%% Build model
mo      = build_oscillator_model(lbsync.xfel.mo,        Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
mlo     = build_laser_model(lbsync.xfel.laser.mlo1,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);
slo     = build_laser_model(lbsync.xfel.laser.slo1,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);
ppl     = build_laser_model(lbsync.xfel.laser.sase3sxp, Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);
lonk    = build_link_model(lbsync.xfel.link.slo1,       Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);
ppllink = build_link_model(lbsync.xfel.link.sase3sxp,   Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);
relonk  = build_link_model(lbsync.xfel.link.relonk,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=bandlimit);

ppl_chain = chain2(mo.Pcol, mlo.Pcol, lonk.Pcol, slo.Pcol, ppllink.Pcol, ppl.Pcol, AnalysisPoint={'r(1)'});
ppl_chain = getIOTransfer(ppl_chain, {'w', 'u'}, {'r(1)', 'y', 'e'});
ppl_chain = connect(ppl_chain, sumblk("z = r(1) - y(1)"), {'w', 'u'}, {'z', 'e'});
ppl_chain = ss(ppl_chain);

rtrip_det = [1, ensure_limits(tf(db2mag(-75), 1), 1e6)];
rtrip_det = chgTimeUnit(rtrip_det, time_unit);
rtrip_det.InputName = ["r", "w"]; rtrip_det.OutputName = "y";
rtrip = chain2(mo.Pcol, mlo.Pcol, lonk.Pcol, slo.Pcol, relonk.Pcol, rtrip_det, AnalysisPoint={'r(2)'});
rtrip = getIOTransfer(rtrip, {'w', 'u'}, {'r(2)', 'y', 'e'});
rtrip = connect(rtrip, sumblk("z = r(2) - y(1)"), {'w', 'u'}, {'z', 'e'});
rtrip = ss(rtrip);

rtrip_bm = chain2(mo.Pcol, mlo.CLcol, lonk.CLcol, slo.CLcol, relonk.CLcol, rtrip_det, AnalysisPoint={'r(2)'});
rtrip_bm = replaceBlock(rtrip_bm, 'e', 1);
rtrip_bm.InputName = 'w';
rtrip_bm = getIOTransfer(rtrip_bm, 'w', {'r(2)', 'y', });
rtrip_bm = connect(rtrip_bm, sumblk("z = r(2) - y(1)"), 'w', 'z');


%% Build reduced-order model
mo_      = build_oscillator_model(lbsync.xfel.mo,        Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
mlo_     = build_laser_model(lbsync.xfel.laser.mlo1,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
lonk_    = build_link_model(lbsync.xfel.link.slo1,       Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
slo_     = build_laser_model(lbsync.xfel.laser.slo1,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
ppllink_ = build_link_model(lbsync.xfel.link.sase3sxp,   Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
ppl_     = build_laser_model(lbsync.xfel.laser.sase3sxp, Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);
relonk_  = build_link_model(lbsync.xfel.link.relonk,     Scaling=scaling, TimeUnit=time_unit, FilterMinDecay=min_decay, Bandlimit=1e6);

ppl_chain_ = chain2(mo_.Pcol, mlo_.Pcol, lonk_.Pcol, slo_.Pcol, ppllink_.Pcol, ppl_.Pcol, AnalysisPoint={'r(1)'});
ppl_chain_ = getIOTransfer(ppl_chain_, {'w', 'u'}, {'r(1)', 'y', 'e'});
ppl_chain_ = connect(ppl_chain_, sumblk("z = r(1) - y(1)"), {'w', 'u'}, {'z', 'e'});
ppl_chain_ = ss(ppl_chain_);

rtrip_det_ = [1, ensure_limits(tf(db2mag(-75), 1), 100e3)];
rtrip_det_ = chgTimeUnit(rtrip_det_, time_unit);
rtrip_det_.InputName = ["r", "w"]; rtrip_det_.OutputName = "y";
rtrip_ = chain2(mo_.Pcol, mlo_.Pcol, lonk_.Pcol, slo_.Pcol, relonk_.Pcol, rtrip_det_, AnalysisPoint={'r(2)'});
rtrip_ = getIOTransfer(rtrip_, {'w', 'u'}, {'r(2)', 'y', 'e'});
rtrip_ = connect(rtrip_, sumblk("z = r(2) - y(1)"), {'w', 'u'}, {'z', 'e'});
rtrip_ = ss(rtrip_);

rtrip_bm_ = chain2(mo_.Pcol, mlo_.CLcol, lonk_.CLcol, slo_.CLcol, relonk_.CLcol, rtrip_det_, AnalysisPoint={'r(2)'});
rtrip_bm_ = replaceBlock(rtrip_bm_, 'e', 1);
rtrip_bm_.InputName = 'w';
rtrip_bm_ = getIOTransfer(rtrip_bm_, 'w', {'r(2)', 'y', });
rtrip_bm_ = connect(rtrip_bm_, sumblk("z = r(2) - y(1)"), 'w', 'z');

%% More setup
switch model
    case "roundtrip"
        sys = rtrip;
        sys_ = rtrip_;
        bm_ctrl = blkdiag(tunablePID("mlo_ctrl", mlo.C), tunablePID("lonk_ctrl", lonk.C), tunablePID("slo_ctrl", slo.C), tunablePID("relonk_ctrl", relonk.C));
        N = 4;
        M = [1 1 1 1 0 0 0 0;
             0 0 0 0 1 0 1 0;
             0 0 0 0 0 1 0 1];
    case "ppl"
        sys = ppl_chain;
        sys_ = ppl_chain_;
        bm_ctrl = blkdiag(tunablePID("mlo_ctrl", mlo.C), tunablePID("lonk_ctrl", lonk.C), tunablePID("slo_ctrl", slo.C), tunablePID("ppllink_ctrl", relonk.C), tunablePID("ppl_ctrl", relonk.C));
        N = 5;
        M = [1 1 0 0 0 0 0 0 0;
             0 0 1 1 1 0 1 0 0;
             0 0 0 0 0 1 0 1 1];
    otherwise
        error("Unknown model '%s'!", model);
end

uidx = startsWith(sys.InputName, 'u'); nu = sum(uidx);
widx = startsWith(sys.InputName, 'w'); nw = sum(widx);
yidx = startsWith(sys.OutputName, 'e'); ny = sum(yidx);
zidx = startsWith(sys.OutputName, 'z'); nz = sum(zidx);
nx = order(sys);

sys_pade = pade(sys, pade_order);
sys_pade_ = pade(sys_, pade_order);
nx_pade = order(sys_pade);
nx_pade_ = order(sys_pade_);

sys = prescale(sys);
sys_pade = prescale(sys_pade);

sys_ = prescale(sys_);
sys_pade_ = prescale(sys_pade_);

%%% Centralized controller synthesis %%% -------------------------------------------------------------------------------
%%% ARE %%%
%% ARE DOF nominal
sol = struct();
h2syn_opts = h2synOptions(Regularize="on");
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn(sys_pade, ny, nu, h2syn_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_are = sol;

%%% LMI %%%
%% LMI DOF nominal
sol = struct();
yalmip_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
h2lmi_opts = h2syn_lmi_options(sdp_options=yalmip_opts, eliminate_variables=false);
h2lmi_opts.tolerance = 0;
h2lmi_opts.minimum_decay = 1e-4;
%h2lmi_opts.maximum_decay = 1e4;
h2lmi_opts.minimum_damping = 1/10;
%h2lmi_opts.maximum_frequency = 2*pi*400;
tmp = balreal(sys_pade);
%tmp = truncate_coeff(sys_pade, 1e-10);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(tmp, ny, nu, h2lmi_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_lmi = sol;

%%% Nonsmooth %%%
%% NS DOF nominal
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=25, Display="final");
ctrl = chgTimeUnit(tunableSS("ctrl", 16, ny, nu), "milliseconds");
CL = lft(sys_pade, ctrl);

[sol.CL, sol.gamma, ~, sol.info] = systune(CL, h2_goal, [], ns_opts);
sol.K = ss(sol.CL.Blocks.ctrl);

print_jitter(sys, sol.K, sol.gamma);
sol_ns = sol;

%% NS PI-OF nominal
sol = struct();
ns_opts = systuneOptions(MaxIter=5e3, RandomStart=0, Display="final");
CL = lft(sys_pade, bm_ctrl);

h2_goal = TuningGoal.Variance("w", "z", 1);
[sol.CL, sol.gamma, ~, sol.info] = systune(CL, h2_goal, [], ns_opts);
sol.K = tf(replaceBlock(bm_ctrl, sol.CL.Blocks));

print_jitter(sys, sol.K, sol.gamma);
sol_ns(2) = sol;

%%% Decentralized controller synthesis %%% -----------------------------------------------------------------------------
%%% LMI %%%
%% SI SOF nominal decentralized
sol = struct();
yalmip_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
h2lmi_opts = h2syn_lmi_options(sdp_options=yalmip_opts);
h2lmi_opts.tolerance = 0;
%h2lmi_opts.minimum_decay = 1e-4;
%h2lmi_opts.minimum_damping = 1/10;

tmp = balreal(sys_pade);
[tmp, Q] = output_state_transform(tmp, 'e', Unitary=false, Orthonormal=false, PreconditionQ=false);
tmp = truncate_coeff(tmp, 1e-10);
sys_pade_so = tmp;

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_si(sys_pade_so, nu, ny, 0, eye(nu, ny), h2lmi_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_si(1) = sol;

%% SI 1st-ord. DOF nominal decentralized
sol = struct();
yalmip_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
h2lmi_opts = h2syn_lmi_options(sdp_options=yalmip_opts);
h2lmi_opts.tolerance = 0;
%h2lmi_opts.minimum_decay = 1e-4;
%h2lmi_opts.minimum_damping = 1/10;

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_si(sys_pade_so, nu, ny, N, kron(ones(2), eye(N)), h2lmi_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_si(2) = sol;

%% SI 2nd-ord. DOF nominal decentralized
sol = struct();
yalmip_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
h2lmi_opts = h2syn_lmi_options(sdp_options=yalmip_opts);
h2lmi_opts.tolerance = 0;

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_si(sys_pade_so, nu, ny, 2*N, kron(ones(3), eye(nu)), h2lmi_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_si(3) = sol;

%%% Nonsmooth %%%
%% NS SOF nominal decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=25, Display="final");
ctrl = tunableGain("ctrl", ny, nu); ctrl.Gain.Free = eye(ny, nu);
ctrl = chgTimeUnit(ctrl, time_unit);
CL = lft(sys_pade, ctrl);

[sol.CL, sol.gamma, ~, sol.info] = systune(CL, h2_goal, [], ns_opts);
sol.K = ss(sol.CL.Blocks.ctrl);

print_jitter(sys, sol.K, sol.gamma);
sol_ns(1) = sol;

%% NS PI-OF nominal decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=25, Display="final");
ctrl = arrayfun(@(i) tunablePID("ctrl" + i, "PI"), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);
CL = lft(sys_pade, ctrl);

[sol.CL, sol.gamma, ~, sol.info] = systune(CL, h2_goal, [], ns_opts);
sol.K = replaceBlock(ctrl, sol.CL.Blocks);

print_jitter(sys, sol.K, sol.gamma);
sol_ns(2) = sol;

%% NS 2nd-ord. DOF nominal decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=25, Display="final");
ctrl = arrayfun(@(i) tunableSS("ctrl" + i, 2, 1, 1), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);
CL = lft(sys_pade, ctrl);

[sol.CL, sol.gamma, ~, sol.info] = systune(CL, h2_goal, [], ns_opts);
sol.K = replaceBlock(ctrl, sol.CL.Blocks);

print_jitter(sys, sol.K, sol.gamma);
sol_ns(3) = sol;

%% Some output
fprintf("Rel. change P->PI:   %.3g\n", rchg(integrate_freqresp(lft(sys, sol_ns(2).K), [1e0, 1e6]), integrate_freqresp(lft(sys, sol_ns(1).K), [1e0, 1e6])));
fprintf("Rel. change PI->2nd: %.3g\n", rchg(integrate_freqresp(lft(sys, sol_ns(3).K), [1e0, 1e6]), integrate_freqresp(lft(sys, sol_ns(2).K), [1e0, 1e6])));
fprintf("Rel. change cent->decent: %.3g\n", rchg(integrate_freqresp(lft(sys, sol_ns(3).K), [1e0, 1e6]), integrate_freqresp(lft(sys_pade, sol_are.K), [1e0, 1e6])));
fprintf("Abs. change cent->decent: %.3g\n", integrate_freqresp(lft(sys, sol_ns(3).K), [1e0, 1e6]) - integrate_freqresp(lft(sys_pade, sol_are.K), [1e0, 1e6]));

%%% Robust decentralized controller synthesis %%% ----------------------------------------------------------------------
%%% Nonsmooth %%%
%% NS PI-OF robust decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=250, UseParallel=true, Display="final");
ctrl = arrayfun(@(i) tunablePID("ctrl" + i, "PI"), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, PerfMultDiag=true, SystuneOpts=ns_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_rob_ns(1) = sol;

%% NS PI-OF robust decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=250, UseParallel=true, Display="final");
ctrl = arrayfun(@(i) tunablePID("ctrl" + i, "PI"), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, PerfMultDiag=true, DlyMultMask=M, SystuneOpts=ns_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_rob_ns(2) = sol;

%% NS 2nd-ord. DOF robust decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=250, UseParallel=true, Display="final");
ctrl = arrayfun(@(i) tunableSS("ctrl" + i, 2, 1, 1), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, PerfMultDiag=true, SystuneOpts=ns_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_rob_ns(3) = sol;

%% NS 2nd-ord. DOF robust decentralized
sol = struct();
ns_opts = systuneOptions(MaxIter=1e3, RandomStart=250, UseParallel=true, Display="final");
ctrl = arrayfun(@(i) tunableSS("ctrl" + i, 2, 1, 1), 1:N, UniformOutput=false);
ctrl = blkdiag(ctrl{:});
ctrl = chgTimeUnit(ctrl, time_unit);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, PerfMultDiag=true, DlyMultMask=M, SystuneOpts=ns_opts);

print_jitter(sys, sol.K, sol.gamma);
sol_rob_ns(4) = sol;

return; % Stop here on Maxwell because of missing sdp solver

%% Some plots
figure(); axes(); hold("on");
scatter([sol_rob_ns(1).info.g], [sol_rob_ns(1).info.f]);
scatter([sol_rob_ns(2).info.g], [sol_rob_ns(2).info.f]);
scatter([sol_rob_ns(3).info.g], [sol_rob_ns(3).info.f]);
xlim([1, 1.02]);
ylim([10, 50]);

%% Try hot-starting with LMI-based multipliers to improve further
ns_opts.RandomStart = 0; ns_opts.UseParallel = false;
ctrl = replaceBlock(ctrl, sol_rob_ns(1).K.Blocks);
[flag, info] = isstable_dly(sol_rob_ns(1).CL, DlyMultOrder=0, LmiTol=0);
dlymult = lmi2tunable_dlymlt(info.sdp_vars);
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, DlyMultInit=dlymult, SystuneOpts=ns_opts);
sol_rob_ns_hs(1) = sol;

ns_opts.RandomStart = 0; ns_opts.UseParallel = false;
ctrl = replaceBlock(ctrl, sol_rob_ns(2).K.Blocks);
[flag, info] = isstable_dly(sol_rob_ns(2).CL, DlyMultOrder=0, LmiTol=0);
dlymult = lmi2tunable_dlymlt(info.sdp_vars);
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, DlyMultInit=dlymult, SystuneOpts=ns_opts);
sol_rob_ns_hs(2) = sol;

% Try hot-starting with LMI-based multipliers to improve further
ns_opts.RandomStart = 0; ns_opts.UseParallel = false;
ctrl = replaceBlock(ctrl, sol_rob_ns(3).K.Blocks);
[flag, info] = isstable_dly(sol_rob_ns(3).CL, DlyMultOrder=0, LmiTol=0);
dlymult = lmi2tunable_dlymlt(info.sdp_vars);
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_ns_tds(sys, ctrl, PerfMultOrder=3, PerfMultReal=true, DlyMultInit=dlymult, SystuneOpts=ns_opts);
sol_rob_ns_hs(3) = sol;

%% Robust synthesis evaluation
load("ns_rob_syn.mat");
%%
load(fullfile(userpath, "Colormaps/batlow/CategoricalPalettes/batlowS.mat"));
figure(); axes(XScale="log", YScale="log"); hold("on");% colororder(batlowS);

for i = 1:4
    t = ns_rob_syn.Info(i).rand_runs;
    t = array2table([1:numel(t); t.g; t.f; t.J]', VariableNames=["idx", "g", "f", "J"]);
    t.f = t.f * sqrt(1e3) / sqrt(2);
    fprintf("Converged: %.3g%%\n", sum(t.g < 1.05) / height(t) * 100);
    t = t(t.g < 1.1, :); t = t(t.J <= 10, :);
    t = sortrows(t, "f");
    if mod(i, 2), mkr = "square"; else, mkr = "o"; end
    scatter(t.f, t.J, 36 ./ rescale(t.g, 1, 12), 'filled', "o");

    fname = sprintf("euxfel_ns_syn_%s_%s", ns_rob_syn.Model(i), lower(ns_rob_syn.CtrlType(i)));
    if ns_rob_syn.DiagPerfMult(i), fname = fname + "_diag"; end
    if ns_rob_syn.GrpDly(i), fname = fname + "_grpdly"; end
    writetable(t, fname + ".dat", delimiter='\t');

    t = t(t.g < 1.01, :);
    [~, optidx] = min(t.f);
    fprintf("Best f=%.3g (J=%g)\n", t.f(optidx), t.J(optidx));
    [~, optidx] = min(t.J);
    fprintf("Best J=%.3g (f=%g)\n", t.J(optidx), t.f(optidx));
end
%xlim([1e2,1e3]);
ylim([2e0,1e1]);


%%
figure(); axes(XScale="log", YScale="log"); hold("on"); %colororder(batlowS);
for i = 5:8
    t = ns_rob_syn.Info(i).rand_runs;
    t = array2table([t.g; t.f; t.J]', VariableNames=["g", "f", "J"]);
    t.f = t.f * sqrt(1e3) / sqrt(2);
    fprintf("Converged: %.3g%%\n", sum(t.g < 1.05) / height(t) * 100);
    t = t(t.g < 1.1, :);
    if mod(i, 2), mkr = "square"; else, mkr = "o"; end
    scatter(t.f, t.J, 36 ./ rescale(t.g, 1, 12), 'filled', "o");

    fname = sprintf("euxfel_ns_syn_%s_%s", ns_rob_syn.Model(i), lower(ns_rob_syn.CtrlType(i)));
    if ns_rob_syn.DiagPerfMult(i), fname = fname + "_diag"; end
    if ns_rob_syn.GrpDly(i), fname = fname + "_grpdly"; end
    writetable(t, fname + ".dat", delimiter='\t');

    t = t(t.g < 1.01, :);
    [~, optidx] = min(t.f);
    fprintf("Best f=%.3g (J=%g)\n", t.f(optidx), t.J(optidx));
    [~, optidx] = min(t.J);
    fprintf("Best J=%.3g (f=%g)\n", t.J(optidx), t.f(optidx));
end
%xlim([1e2,1e3]);
ylim([2e0,1e1]);

%%
figure(); axes(XScale="log", YScale="log"); hold("on"); %colororder(batlowS);
for i = 9:12
    t = ns_rob_syn.Info(i).rand_runs;
    t = array2table([t.g; t.f; t.J]', VariableNames=["g", "f", "J"]);
    t.f = t.f * sqrt(1e3) / sqrt(2);
    fprintf("Converged: %.3g%%\n", sum(t.g < 1.05) / height(t) * 100);
    t = t(t.g < 1.1, :);
    if mod(i, 2), mkr = "square"; else, mkr = "o"; end
    scatter(t.f, t.J, 36 ./ rescale(t.g, 1, 12), 'filled', "o");

    fname = sprintf("euxfel_ns_syn_%s_%s", ns_rob_syn.Model(i), lower(ns_rob_syn.CtrlType(i)));
    if ns_rob_syn.DiagPerfMult(i), fname = fname + "_diag"; end
    if ns_rob_syn.GrpDly(i), fname = fname + "_grpdly"; end
    writetable(t, fname + ".dat", delimiter='\t');

    t = t(t.g < 1.01, :);
    [~, optidx] = min(t.f);
    fprintf("Best f=%.3g (J=%g)\n", t.f(optidx), t.J(optidx));
    [~, optidx] = min(t.J);
    fprintf("Best J=%.3g (f=%g)\n", t.J(optidx), t.f(optidx));
end
%xlim([1e2,1e3]);
ylim([5e-1,1e1]);

figure(); axes(XScale="log", YScale="log"); hold("on"); %colororder(batlowS);
for i = 13:16
    t = ns_rob_syn.Info(i).rand_runs;
    t = array2table([t.g; t.f; t.J]', VariableNames=["g", "f", "J"]);
    t.f = t.f * sqrt(1e3) / sqrt(2);
    fprintf("Converged: %.3g%%\n", sum(t.g < 1.05) / height(t) * 100);
    t = t(t.g < 1.1, :);
    if mod(i, 2), mkr = "square"; else, mkr = "o"; end
    scatter(t.f, t.J, 36 ./ rescale(t.g, 1, 12), 'filled', "o");

    fname = sprintf("euxfel_ns_syn_%s_%s", ns_rob_syn.Model(i), lower(ns_rob_syn.CtrlType(i)));
    if ns_rob_syn.DiagPerfMult(i), fname = fname + "_diag"; end
    if ns_rob_syn.GrpDly(i), fname = fname + "_grpdly"; end
    writetable(t, fname + ".dat", delimiter='\t');

    t = t(t.g < 1.01, :);
    [~, optidx] = min(t.f);
    fprintf("Best f=%.3g (J=%g)\n", t.f(optidx), t.J(optidx));
    [~, optidx] = min(t.J);
    fprintf("Best J=%.3g (f=%g)\n", t.J(optidx), t.f(optidx));
end
%xlim([1e2,1e3]);
ylim([5e-1,1e1]);

TileFigures();

%% Plot best nominal decentralized controller vs robust controller with best nominal perf. and best robust perf.
f = logspace(0, 4, 100)';
%figure();

K = sol.K; %bodemag(K); hold("on");
K = [K(1,1), K(2,2), K(3,3), K(4,4), K(5,5)];
tbl = array2table([f, mag2db(squeeze(abs(freqresp(K, f, "Hz"))))'], VariableNames=["Frequency", "Ctrl1", "Ctrl2", "Ctrl3", "Ctrl4", "Ctrl5"]);
figure(); axes(XScale="log"); hold("on"); plot(tbl, "Frequency", 2:6);
tbl = reducepoly_table(tbl, 1e-2);
for i = 2:6, tmp = rmmissing(tbl{:,[1,i]}); disp(size(tmp,1)); plot(tmp(:,1), tmp(:,2), 'k:'); end
writetable(tbl, "xfel_lbsync_ppl_ctrl_nom.dat", Delimiter='\t');

K = replaceBlock(ns_rob_syn.K{1}, ns_rob_syn.Info(1).rand_runs(40).Blocks); %bodemag(K);
K = [K(1,1), K(2,2), K(3,3), K(4,4), K(5,5)];
tbl = array2table([f, mag2db(squeeze(abs(freqresp(K, f, "Hz"))))'], VariableNames=["Frequency", "Ctrl1", "Ctrl2", "Ctrl3", "Ctrl4", "Ctrl5"]);
figure(); axes(XScale="log"); hold("on"); plot(tbl, "Frequency", 2:6);
tbl = reducepoly_table(tbl, 1e-2);
for i = 2:6, tmp = rmmissing(tbl{:,[1,i]}); disp(size(tmp,1)); plot(tmp(:,1), tmp(:,2), 'k:'); end
writetable(tbl, "xfel_lbsync_ppl_ctrl_rob.dat", Delimiter='\t');

K = replaceBlock(ns_rob_syn.K{3}, ns_rob_syn.Info(3).rand_runs(11).Blocks); %bodemag(K);
K = [K(1,1), K(2,2), K(3,3), K(4,4), K(5,5)];
tbl = array2table([f, mag2db(squeeze(abs(freqresp(K, f, "Hz"))))'], VariableNames=["Frequency", "Ctrl1", "Ctrl2", "Ctrl3", "Ctrl4", "Ctrl5"]);
figure(); axes(XScale="log"); hold("on"); plot(tbl, "Frequency", 2:6);
tbl = reducepoly_table(tbl, 1e-2);
for i = 2:6, tmp = rmmissing(tbl{:,[1,i]}); disp(size(tmp,1)); plot(tmp(:,1), tmp(:,2), 'k:'); end
writetable(tbl, "xfel_lbsync_ppl_ctrl_robnom.dat", Delimiter='\t');


%%

sol = sol_rob_ns(1); % PI
for i = 1:numel(sol.info.rand_runs)
    sol.info.rand_runs(i).J = integrate_freqresp(ss(lft(sys, replaceBlock(sol.K, sol.info.rand_runs(i).Blocks))), [1e0, 1e6]);
end
t = [1:numel(sol.info.rand_runs); [sol.info.rand_runs.g]; [sol.info.rand_runs.f]; [sol.info.rand_runs.J]]';
t = array2table(t, VariableNames=["idx", "g", "f", "J"]);

tmp = t(t.g < 1.1, :);
scatter(tmp.f * sqrt(1e3) / sqrt(2), tmp.J, 36 ./ rescale(tmp.g, 1, 12), 'filled');

%%
ylim([1e0, 1e2]);

%%
[~, optidx] = min(tmp.f);
%%
PsiY = sol.info.PsiY;

figure(); bodemag(PsiY, replaceBlock(PsiY, sol.info.rand_runs(optidx).Blocks));

%% END
return;

%%% Functions %%% ------------------------------------------------------------------------------------------------------

function print_jitter(sys, ctrl, gamma, pade_order, range)
    arguments
        sys lti
        ctrl lti
        gamma (1,1) double = nan
        pade_order (1,1) {mustBeInteger,mustBePositive,mustBeFinite} = 1
        range (2,1) double {mustBeNonnegative,mustBeFinite} = [1e0, 1e6]
    end

    cl = lft(sys, ctrl);
    cl_pade = pade(cl, pade_order);

    if isfinite(gamma)
        fprintf("Guaranteed performance: %.3g\n", gamma * sqrt(1e3) / sqrt(2));
    end
    if isstable(cl_pade)
        fprintf("Padé-approximated performance: %.3g\n", norm(cl_pade) * sqrt(1e3) / sqrt(2));
    else
        fprintf("Warning: System not stable!")
    end
    fprintf("Integrated jitter 1Hz-1MHz: %.3g\n", integrate_freqresp(cl_pade, range));
    fprintf("Integrated jitter 1Hz-1MHz: %.3g\n", integrate_freqresp(cl, range));
end

function dlymult = lmi2tunable_dlymlt(sdp_vars)
    fnames = string(fieldnames(sdp_vars));
    dlymult = struct();
    for fname = fnames'
        if startsWith(fname, "D") || startsWith(fname, "R")
            lmivar = sdp_vars.(fname);
            assert(issymmetric(lmivar), "Valid delay multiplier variables must be symmetric!");
            [m, ~] = size(lmivar);
            try
                R = chol(lmivar);
                for i = 1:m
                    for j = 1:i
                        dlymult.(fname + "_" + i + j) = realp(fname + "_" + i + j, value(R(i, j)));
                    end
                end
            catch
                % TODO
            end
        end
    end
end

%%
dlymult = lmi2tunable_dlymlt(info.sdp_vars);
%%


%%% Code Dump %%% ------------------------------------------------------------------------------------------------------


%% - SI 1st-order dynamic nominal decentralized
sys_pade_aug_ = blkdiag(sys_pade_, prescale(chgTimeUnit(ss(zeros(nu), eye(nu), eye(nu), [], InputName='c', OutputName='c'), "milliseconds")));
nx = order(sys_pade_aug_);
xpermt = 1:nx;
tmp = xpermt((end-2*nu+1):end);
xpermt((end-2*nu)+1:end) = reshape(reshape(tmp, [], 2)', 1, []);
sys_pade_aug_ = xperm(sys_pade_aug_, xpermt);

upermt = 1:size(sys_pade_aug_, 2);
tmp = upermt((end-2*nu+1):end);
upermt((end-2*nu+1):end) = reshape(reshape(tmp, [], 2)', 1, []);
ypermt = 1:size(sys_pade_aug_, 1);
tmp = ypermt((end-2*nu+1):end);
ypermt((end-2*nu+1):end) = reshape(reshape(tmp, [], 2)', 1, []);

Kipermt = 1:2*nu;
Kipermt(ypermt(2:end)-1) = Kipermt;

sys_pade_aug_ = sys_pade_aug_(ypermt, upermt);

[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_si(sys_pade_aug_, 2*nu, 2*ny, kron(eye(nu), ones(2)), sdp_options=yalmip_opts);

sol.K = sol.K(Kipermt,Kipermt);
sol.K = ss(sol.K(1:nu,1:nu), sol.K(nu+1:2*nu,1:nu), sol.K(1:nu,nu+1:2*nu), sol.K(nu+1:2*nu,nu+1:2*nu), TimeUnit=time_unit);

%%
tmp1 = load("ns_rob_syn_real_ppl_2025-01-29.mat", "sol_rob_ns*");
tmp2 = load("ns_rob_syn_diag_ppl_2025-01-29.mat", "sol_rob_ns*");
tmp3 = load("ns_rob_syn_real_rtrip_2025-01-30.mat", "sol_rob_ns*");
tmp4 = load("ns_rob_syn_diag_rtrip_2025-01-30.mat", "sol_rob_ns*");

t = {
    "ppl", "PI" , false, false, tmp1.sol_rob_ns(1).K, tmp1.sol_rob_ns(1).CL, tmp1.sol_rob_ns(1).gamma, tmp1.sol_rob_ns(1).info;
    "ppl", "PI" , false, true , tmp1.sol_rob_ns(2).K, tmp1.sol_rob_ns(2).CL, tmp1.sol_rob_ns(2).gamma, tmp1.sol_rob_ns(2).info;
    "ppl", "PI" , true , false, tmp2.sol_rob_ns(1).K, tmp2.sol_rob_ns(1).CL, tmp2.sol_rob_ns(1).gamma, tmp2.sol_rob_ns(1).info;
    "ppl", "PI" , true , true , tmp2.sol_rob_ns(2).K, tmp2.sol_rob_ns(2).CL, tmp2.sol_rob_ns(2).gamma, tmp2.sol_rob_ns(2).info;
    "ppl", "dof", false, false, tmp1.sol_rob_ns(3).K, tmp1.sol_rob_ns(3).CL, tmp1.sol_rob_ns(3).gamma, tmp1.sol_rob_ns(3).info;
    "ppl", "dof", false, true , tmp1.sol_rob_ns(4).K, tmp1.sol_rob_ns(4).CL, tmp1.sol_rob_ns(4).gamma, tmp1.sol_rob_ns(4).info;
    "ppl", "dof", true , false, tmp2.sol_rob_ns(3).K, tmp2.sol_rob_ns(3).CL, tmp2.sol_rob_ns(3).gamma, tmp2.sol_rob_ns(3).info;
    "ppl", "dof", true , true , tmp2.sol_rob_ns(4).K, tmp2.sol_rob_ns(4).CL, tmp2.sol_rob_ns(4).gamma, tmp2.sol_rob_ns(4).info;
    "rtrip", "PI" , false, false, tmp3.sol_rob_ns(1).K, tmp3.sol_rob_ns(1).CL, tmp3.sol_rob_ns(1).gamma, tmp3.sol_rob_ns(1).info;
    "rtrip", "PI" , false, true , tmp3.sol_rob_ns(2).K, tmp3.sol_rob_ns(2).CL, tmp3.sol_rob_ns(2).gamma, tmp3.sol_rob_ns(2).info;
    "rtrip", "PI" , true , false, tmp4.sol_rob_ns(1).K, tmp4.sol_rob_ns(1).CL, tmp4.sol_rob_ns(1).gamma, tmp4.sol_rob_ns(1).info;
    "rtrip", "PI" , true , true , tmp4.sol_rob_ns(2).K, tmp4.sol_rob_ns(2).CL, tmp4.sol_rob_ns(2).gamma, tmp4.sol_rob_ns(2).info;
    "rtrip", "dof", false, false, tmp3.sol_rob_ns(3).K, tmp3.sol_rob_ns(3).CL, tmp3.sol_rob_ns(3).gamma, tmp3.sol_rob_ns(3).info;
    "rtrip", "dof", false, true , tmp3.sol_rob_ns(4).K, tmp3.sol_rob_ns(4).CL, tmp3.sol_rob_ns(4).gamma, tmp3.sol_rob_ns(4).info;
    "rtrip", "dof", true , false, tmp4.sol_rob_ns(3).K, tmp4.sol_rob_ns(3).CL, tmp4.sol_rob_ns(3).gamma, tmp4.sol_rob_ns(3).info;
    "rtrip", "dof", true , true , tmp4.sol_rob_ns(4).K, tmp4.sol_rob_ns(4).CL, tmp4.sol_rob_ns(4).gamma, tmp4.sol_rob_ns(4).info;
}
t = cell2table(t, VariableNames=["Model", "CtrlType", "DiagPerfMult", "GrpDly", "K", "CL", "Gamma", "Info"]);

%%
for i = 1:height(ns_rob_syn)
    for j = 1:250
        try
            if ns_rob_syn.Model(i) == "ppl"
                ns_rob_syn.Info(i).rand_runs(j).J = integrate_freqresp(lft(ppl_chain, replaceBlock(ns_rob_syn.K{i}, ns_rob_syn.Info(i).rand_runs(j).Blocks)), [1e0,1e6]);
            else
                ns_rob_syn.Info(i).rand_runs(j).J = integrate_freqresp(lft(rtrip, replaceBlock(ns_rob_syn.K{i}, ns_rob_syn.Info(i).rand_runs(j).Blocks)), [1e0,1e6]);
            end
            if ns_rob_syn.Info(i).rand_runs(j).J <= 0
                ns_rob_syn.Info(i).rand_runs(j).J = Inf;
            end
        catch
            ns_rob_syn.Info(i).rand_runs(j).J = Inf;
        end
    end
end