% ### Laser PLL Phase Tracking Simulation ###
%
% SPDX-FileCopyrightText: 2023 Deutsches Elektronen-Synchrotron DESY, MSK, LbSync <lbsync-expert@desy.de>
% SPDX-License-Identifier: GPL-3.0-or-later
%
% Author: Maximilian Schütte <maximilian.schuette@desy.de>
%
% Control studies for VCO PLL phase tracking.
yalmip('clear'); clear;


drv_bw = 25e3; % 25 kHz driver bandwidth (typical DESY PZT4)
drv_k = 40; % 40 V/V gain (typical PZT4 gain in unipolar mode)
vco_k = 250; % (2*pi * k_omega=6.5 * n_harm=6) ~= 250

% Plant model in rad @ 1.3GHz
% !!! Scaling Model here leads to better numerical results, remember to scale controller accordingly.
ctrl_scale = 1e-3;
G = zpk([], [0, -2*pi*drv_bw], (2*pi*drv_bw) * drv_k * vco_k);

% Noise filter blocks in mrad @ 1.3GHz
Fr = zpk([], 0, 2);
Fd = zpk(-2*pi*1e5, [0, -2*pi*5], 500 / (2*pi*1e5) * (2*pi*5));
Fn = tf(db2mag(-100));

[P, Pinfo] = build_pll_model(G, Fr, Fd, Fn, Rolloff=1, TimeUnit="milliseconds");
% Ignore phase output
P = P(Pinfo.zidx | Pinfo.eidx, :);
P = ss(zpk(P)); % better than minreal
P = prescale(P);
P = balreal(P);

%unpackStruct(Pinfo);
ny = Pinfo.ny; nu = Pinfo.nu;

[~, ~, ~, Preg] = h2syn(P, ny, nu); Preg = Preg.Preg;  % See how h2syn regularizes the plant


%- Toolbox ARE / eps-perturbation method
h2syn_opts = h2synOptions();
sol = struct();
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn(P, ny, nu, h2syn_opts);
fprintf("Toolbox h2syn gamma = %g\n", sol.gamma);

sol_are = sol;
clear sol;


%- LMI method
sol = struct();
sdp_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);
%sdp_opts.mosek.MSK_DPAR_INTPNT_CO_TOL_REL_GAP = 1e-12;
%sdp_opts.mosek.MSK_DPAR_INTPNT_TOL_PATH = 1e-5;

h2lmi_opts = h2syn_lmi_options(eliminate_variables=false, sdp_options=sdp_opts, tolerance=1e-9, scale=false);
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(P, ny, nu, h2lmi_opts);
sol_lmi(1) = sol;

h2lmi_opts.minimum_damping = sqrt(2)/10;
h2lmi_opts.tolerance = 0;
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(P, ny, nu, h2lmi_opts);
sol_lmi(2) = sol;

h2lmi_opts.maximum_decay = pi*1e2;
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(P, ny, nu, h2lmi_opts);
sol_lmi(3) = sol;

h2lmi_opts.minimum_decay = pi*1e-3;
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(P, ny, nu, h2lmi_opts);
sol_lmi(4) = sol;

h2lmi_opts.regularization_factor = 2;
h2lmi_opts.relaxation_factor = 1.5;
[sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(P, ny, nu, h2lmi_opts);
sol_lmi(5) = sol;

fprintf("LMI gamma summary: %s\n", num2str([sol_lmi(:).gamma]));
clear sol;

%- Nonsmooth tuning method
h2goal = TuningGoal.Variance('w', 'z', 1);
systopt = systuneOptions(RandomStart=10, Display='off');
% systopt.RandomStart = 10;

sol_ns_tf = struct('CL', {}, 'gamma', {}, 'K', {});
for np = 3:-1:0
    for nz = np:-1:0
        ctrl = tunableTF('ctrl', nz, np);
        ctrl = chgTimeUnit(ctrl, P.TimeUnit);
        [sol.CL, sol.gamma] = systune(lft(P, ctrl), h2goal, systopt);
        sol.K = ss(sol.CL.Blocks.ctrl);
        sol_ns_tf(end+1) = sol; %#ok<SAGROW> 
    end
end; clear sol;
fprintf("Nonsmooth TF ctrl tuning gamma summary: %s\n", num2str([sol_ns_tf(:).gamma]));

sol_ns_ss = struct('CL', {}, 'gamma', {}, 'K', {});
for nx = 4:-1:1  % 0th-order covered py p-controller
    ctrl = tunableSS('ctrl', nx, 1, 1);
    ctrl = chgTimeUnit(ctrl, P.TimeUnit);
    [sol.CL, sol.gamma] = systune(lft(P, ctrl), h2goal, systopt);
    sol.K = ss(sol.CL.Blocks.ctrl);
    sol_ns_ss(end+1) = sol; %#ok<SAGROW> 
end; clear sol;
fprintf("Nonsmooth TF SS tuning gamma summary: %s\n", num2str([sol_ns_ss(:).gamma]));

sol_ns_pid = struct('CL', {}, 'gamma', {}, 'K', {});
for type = ["P", "PI", "PD", "PID"]
    ctrl = tunablePID('ctrl', char(type));
    ctrl = chgTimeUnit(ctrl, P.TimeUnit);
    [sol.CL, sol.gamma] = systune(lft(P, ctrl), h2goal, systopt);
    sol.K = pid(sol.CL.Blocks.ctrl);
    sol_ns_pid(end+1) = sol; %#ok<SAGROW> 
end; clear sol;
fprintf("Nonsmooth PID tuning gamma summary: %s\n", num2str([sol_ns_pid(:).gamma]));


%%
h2inf_warn = warning("off", "Control:analysis:NormInfinite3");

%- P grid scan
KpOpt = sol_ns_pid(1).K.Kp;
DeltaKpGrid = -12:0.5:12;
KpGrid = KpOpt .* db2mag(DeltaKpGrid);
gamma = zeros(size(KpGrid));

for i = 1:numel(KpGrid)
    gamma(i) = norm(lft(P, pid(KpGrid(i), TimeUnit=P.TimeUnit)));
end; clear i;
gamma(isinf(gamma)) = nan;
sol_p = struct(KpOpt=KpOpt, KpGrid=KpGrid, DeltaKpGrid=DeltaKpGrid, gamma=gamma);

%- PI grid scan
KpOpt = sol_ns_pid(2).K.Kp;
KiOpt = sol_ns_pid(2).K.Ki;
[DeltaKpGrid, DeltaKiGrid] = meshgrid(-20:0.5:20, -20:0.5:20);
KpGrid = KpOpt .* db2mag(DeltaKpGrid);
KiGrid = KiOpt .* db2mag(DeltaKiGrid);
gamma = zeros(size(KpGrid));

for i = 1:numel(KpGrid)
    gamma(i) = norm(lft(P, pid(KpGrid(i), KiGrid(i) ./ KpGrid(i), TimeUnit=P.TimeUnit)));
end; clear i;
gamma(isinf(gamma)) = nan;
sol_pi = struct(KpOpt=KpOpt, KiOpt=KiOpt, KpGrid=KpGrid, KiGrid=KiGrid, DeltaKpGrid=DeltaKpGrid, DeltaKiGrid=DeltaKiGrid, gamma=gamma);

%- PD grid scan
KpOpt = sol_ns_pid(3).K.Kp;
KdOpt = sol_ns_pid(3).K.Kd;
[DeltaKpGrid, DeltaKdGrid] = meshgrid(-20:0.5:20, -20:0.5:20);
KpGrid = KpOpt .* db2mag(DeltaKpGrid);
KdGrid = KiOpt .* db2mag(DeltaKdGrid);
gamma = zeros(size(KpGrid));

for i = 1:numel(KpGrid)
    gamma(i) = norm(lft(P, pid(KpGrid(i), 0, KdGrid(i), TimeUnit=P.TimeUnit)));
end; clear i;
gamma(isinf(gamma)) = nan;
sol_pd = struct(KpOpt=KpOpt, KdOpt=KiOpt, KpGrid=KpGrid, KdGrid=KdGrid, DeltaKpGrid=DeltaKpGrid, DeltaKdGrid=DeltaKdGrid, gamma=gamma);

warning(h2inf_warn); % Restore state of Norm infinite warning

%% Output data
Ktmp = zpk(sol_are.K);
Kdcgain = dcgain(Ktmp);
Ktmp.Z{1} = Ktmp.Z{1}(1:2);
Ktmp.P{1} = Ktmp.P{1}(3:4);
Ktmp.K = Ktmp.K / dcgain(Ktmp) * Kdcgain;
sol_are.K_reduced = Ktmp;
clear Ktmp Kdcgain;%.2f

% Performance Numbers:
perf_tbl = table(Size=[10,3], VariableTypes=["double", "double", "double"], ...
                 VariableNames=["H2-Norm", "Timing Jitter", "Optimality Gap"], ...
                 RowNames=["ARE", ...
                           "ARE (implementable)", ...
                           "LMI", ...
                           "LMI (pole-region cnst'd)", ...
                           "Nonsmooth (2nd-order)", ...
                           "Nonsmooth (1st-order)", ...
                           "Nonsmooth (P)", ...
                           "Nonsmooth (PI)", ...
                           "Nonsmooth (PD)", ...
                           "Nonsmooth (PID)"]);

perf_tbl{"ARE", "H2-Norm"} = norm(sol_are.CL);
perf_tbl{"ARE (implementable)", "H2-Norm"} = norm(lft(P, sol_are.K_reduced));
perf_tbl{"LMI", "H2-Norm"} = norm(sol_lmi(1).CL);
perf_tbl{"LMI (pole-region cnst'd)", "H2-Norm"} = norm(sol_lmi(4).CL);
perf_tbl{"Nonsmooth (2nd-order)", "H2-Norm"} = norm(sol_ns_ss(3).CL);
perf_tbl{"Nonsmooth (1st-order)", "H2-Norm"} = norm(sol_ns_ss(4).CL);
perf_tbl{"Nonsmooth (P)", "H2-Norm"} = norm(sol_ns_pid(1).CL);
perf_tbl{"Nonsmooth (PI)", "H2-Norm"} = norm(sol_ns_pid(2).CL);
perf_tbl{"Nonsmooth (PD)", "H2-Norm"} = norm(sol_ns_pid(3).CL);
perf_tbl{"Nonsmooth (PID)", "H2-Norm"} = norm(sol_ns_pid(4).CL);

% Explanation: Multiply by sqrt(1e3) to compensate model scaled in milliseconds
perf_tbl.("Timing Jitter") = perf_tbl.("H2-Norm") * sqrt(1e3) / (2*pi*1.3e9) * 1e12; % divide by 1e3 to convert to rad, then by 2*pi*f0 to seconds, then by 1e15 to fs.
perf_tbl.("H2-Norm") = perf_tbl.("H2-Norm") * sqrt(1e3) * 1e3; % multiply by 1e3 to convert to urad
best = min(perf_tbl.("H2-Norm"));
perf_tbl.("Optimality Gap") = (perf_tbl.("H2-Norm") - best) ./ best * 100;
perf_tbl.Properties.VariableUnits = ["urad", "fs", "percent"];

disp(perf_tbl);

%% Plot data
clrs = colororder();
SHOW_POLYLINE_COMPRESSION = false;

% Figure 1: Magnitude response of G
f = logspace(-2, 6, 2^10)';
f_lg = linspace(-2, 6, 2^10)';

G_mag_resp = mag2db(abs(squeeze(freqresp(Pinfo.G, f, "Hz"))));
pll_ex_g = array2table(reducepoly([f_lg, G_mag_resp], 5e-5), VariableNames=["f", "g"]);
pll_ex_g.f = 10.^(pll_ex_g.f);
writetable(pll_ex_g, "pll_ex_g.dat", Delimiter='\t');

fig = figure();
ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1]); hold("on");

if SHOW_POLYLINE_COMPRESSION
    plot(f, G_mag_resp, Color='k'); scatter(pll_ex_g.f, pll_ex_g.g, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(pll_ex_g.f, pll_ex_g.g, Tag="l:pll_ex:g");

ylim([-100, 120]);

xlabel("Frequency (Hz)");
ylabel("Magnitude (dB)");

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_g.pdf", ContentType="vector");

%% Figure 2: Magnitude responses of Fr Fd and Fn
f = logspace(-2, 6, 2^10)';
f_lg = linspace(-2, 6, 2^10)';
Fr_mag_resp = mag2db(abs(squeeze(freqresp(Pinfo.Fr, f, "Hz"))));
Fd_mag_resp = mag2db(abs(squeeze(freqresp(Pinfo.Fd, f, "Hz"))));
Fn_mag_resp = mag2db(abs(squeeze(freqresp(Pinfo.Fn, f, "Hz"))));

pll_ex_fr = array2table(reducepoly([f_lg, Fr_mag_resp], 5e-5), VariableNames=["f", "fr"]);
pll_ex_fr.f = 10.^(pll_ex_fr.f);
writetable(pll_ex_fr, "pll_ex_fr.dat", Delimiter='\t');
pll_ex_fd = array2table(reducepoly([f_lg, Fd_mag_resp], 5e-5), VariableNames=["f", "fd"]);
pll_ex_fd.f = 10.^(pll_ex_fd.f);
writetable(pll_ex_fd, "pll_ex_fd.dat", Delimiter='\t');
pll_ex_fn = array2table(reducepoly([f_lg, Fn_mag_resp], 1), VariableNames=["f", "fn"]);
pll_ex_fn.f = 10.^(pll_ex_fn.f);
writetable(pll_ex_fn, "pll_ex_fn.dat", Delimiter='\t');

fig = figure();
ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1]); hold("on");

if SHOW_POLYLINE_COMPRESSION
    plot(f, Fr_mag_resp, 'k'); scatter(pll_ex_fr.f, pll_ex_fr.fr, [], 'k');
    plot(f, Fd_mag_resp, 'k'); scatter(pll_ex_fd.f, pll_ex_fd.fd, [], 'k');
    plot(f, Fn_mag_resp, 'k'); scatter(pll_ex_fn.f, pll_ex_fn.fn, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(pll_ex_fr.f, pll_ex_fr.fr, Tag="l:pll_ex:fr");
plot(pll_ex_fd.f, pll_ex_fd.fd, Tag="l:pll_ex:fd");
plot(pll_ex_fn.f, pll_ex_fn.fn, Tag="l:pll_ex:fn");

ylim([-180, 50]);

xlabel("Offset Frequency (Hz)");
ylabel("Phase noise PSD (dB mrad^2 / Hz)", Interpreter='tex');

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_fr_fd_fn.pdf", ContentType="vector");

%% Figure 3: LMI controller responses
f = logspace(-2, 10, 2^10)';
f_lg = linspace(-2, 10, 2^10)';

K_are_mag_resp = mag2db(abs(squeeze(freqresp(sol_are.K, f, "Hz"))) .* ctrl_scale);
K_lmi_mag_resp = mag2db(abs(squeeze(freqresp(sol_lmi(1).K, f, "Hz"))) .* ctrl_scale);
K_cnst_lmi_mag_resp = mag2db(abs(squeeze(freqresp(sol_lmi(4).K, f, "Hz"))) .* ctrl_scale);

pll_ex_are_ctrl = array2table(reducepoly([f_lg, K_are_mag_resp], 5e-5), VariableNames=["f", "are_ctrl"]);
pll_ex_are_ctrl.f = 10.^(pll_ex_are_ctrl.f);
writetable(pll_ex_are_ctrl, "pll_ex_are_ctrl.dat", Delimiter='\t');
pll_ex_lmi_ctrl = array2table(reducepoly([f_lg, K_lmi_mag_resp], 5e-5), VariableNames=["f", "lmi_ctrl"]);
pll_ex_lmi_ctrl.f = 10.^(pll_ex_lmi_ctrl.f);
writetable(pll_ex_lmi_ctrl, "pll_ex_lmi_ctrl.dat", Delimiter='\t');
pll_ex_cnst_lmi_ctrl = array2table(reducepoly([f_lg, K_cnst_lmi_mag_resp], 5e-5), VariableNames=["f", "cnst_lmi_ctrl"]);
pll_ex_cnst_lmi_ctrl.f = 10.^(pll_ex_cnst_lmi_ctrl.f);
writetable(pll_ex_cnst_lmi_ctrl, "pll_ex_cnst_lmi_ctrl.dat", Delimiter='\t');

fig = figure();
ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1]); hold("on");

if SHOW_POLYLINE_COMPRESSION
    plot(f, K_are_mag_resp, 'k'); scatter(pll_ex_are_ctrl.f, pll_ex_are_ctrl.are_ctrl, [], 'k');
    plot(f, K_lmi_mag_resp, 'k'); scatter(pll_ex_lmi_ctrl.f, pll_ex_lmi_ctrl.lmi_ctrl, [], 'k');
    plot(f, K_cnst_lmi_mag_resp, 'k'); scatter(pll_ex_cnst_lmi_ctrl.f, pll_ex_cnst_lmi_ctrl.cnst_lmi_ctrl, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(pll_ex_are_ctrl.f, pll_ex_are_ctrl.are_ctrl, Tag="l:pll_ex:are_ctrl");
plot(pll_ex_lmi_ctrl.f, pll_ex_lmi_ctrl.lmi_ctrl, Tag="l:pll_ex:lmi_ctrl");
plot(pll_ex_cnst_lmi_ctrl.f, pll_ex_cnst_lmi_ctrl.cnst_lmi_ctrl, Tag="l:pll_ex:cnst_lmi_ctrl");

ylim([-50, 50]);

xlabel("Frequency (Hz)");
ylabel("Magnitude (dB)");

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_lmi_ctrl.pdf", ContentType="vector");


%% Figure 4: SS controller responses
f = logspace(-2, 10, 2^10)';
f_lg = linspace(-2, 10, 2^10)';

K_ss_2nd_mag_resp = mag2db(abs(squeeze(freqresp(sol_ns_ss(3).K, f, "Hz"))) .* ctrl_scale);
K_ss_1st_mag_resp = mag2db(abs(squeeze(freqresp(sol_ns_ss(4).K, f, "Hz"))) .* ctrl_scale);

pll_ex_ss_2nd_ctrl = array2table(reducepoly([f_lg, K_ss_2nd_mag_resp], 1e-4), VariableNames=["f", "ss_2nd_ctrl"]);
pll_ex_ss_2nd_ctrl.f = 10.^(pll_ex_ss_2nd_ctrl.f);
writetable(pll_ex_ss_2nd_ctrl, "pll_ex_ss_2nd_ctrl.dat", Delimiter='\t');
pll_ex_ss_1st_ctrl = array2table(reducepoly([f_lg, K_ss_1st_mag_resp], 1e-4), VariableNames=["f", "ss_1st_ctrl"]);
pll_ex_ss_1st_ctrl.f = 10.^(pll_ex_ss_1st_ctrl.f);
writetable(pll_ex_ss_1st_ctrl, "pll_ex_ss_1st_ctrl.dat", Delimiter='\t');

fig = figure();
ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1]); hold("on");

if SHOW_POLYLINE_COMPRESSION
    plot(f, K_ss_2nd_mag_resp, 'k'); scatter(pll_ex_ss_2nd_ctrl.f, pll_ex_ss_2nd_ctrl.ss_2nd_ctrl, [], 'k');
    plot(f, K_ss_1st_mag_resp, 'k'); scatter(pll_ex_ss_1st_ctrl.f, pll_ex_ss_1st_ctrl.ss_1st_ctrl, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(pll_ex_ss_2nd_ctrl.f, pll_ex_ss_2nd_ctrl.ss_2nd_ctrl, Tag="l:pll_ex:ss_2nd_ctrl");
plot(pll_ex_ss_1st_ctrl.f, pll_ex_ss_1st_ctrl.ss_1st_ctrl, Tag="l:pll_ex:ss_1st_ctrl");
plot(pll_ex_are_ctrl.f, pll_ex_are_ctrl.are_ctrl, "--k", Tag="l:pll_ex:benchmark");

xlabel("Frequency (Hz)");
ylabel("Magnitude (dB)");

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_ss_ctrl.pdf", ContentType="vector");

%% Figure 5: PID controller responses
f = logspace(-2, 10, 2^10)';
f_lg = linspace(-2, 10, 2^10)';

K_p_mag_resp = mag2db(abs(squeeze(freqresp(sol_ns_pid(1).K, f, "Hz"))) .* ctrl_scale);
K_pi_mag_resp = mag2db(abs(squeeze(freqresp(sol_ns_pid(2).K, f, "Hz"))) .* ctrl_scale);
K_pid_mag_resp = mag2db(abs(squeeze(freqresp(sol_ns_pid(4).K, f, "Hz"))) .* ctrl_scale);

pll_ex_p_ctrl = array2table(reducepoly([f_lg, K_p_mag_resp], 1e-4), VariableNames=["f", "p_ctrl"]);
pll_ex_p_ctrl.f = 10.^(pll_ex_p_ctrl.f);
writetable(pll_ex_p_ctrl, "pll_ex_p_ctrl.dat", Delimiter='\t');
pll_ex_pi_ctrl = array2table(reducepoly([f_lg, K_pi_mag_resp], 1e-4), VariableNames=["f", "pi_ctrl"]);
pll_ex_pi_ctrl.f = 10.^(pll_ex_pi_ctrl.f);
writetable(pll_ex_pi_ctrl, "pll_ex_pi_ctrl.dat", Delimiter='\t');
pll_ex_pid_ctrl = array2table(reducepoly([f_lg, K_pid_mag_resp], 1e-4), VariableNames=["f", "pi_ctrl"]);
pll_ex_pid_ctrl.f = 10.^(pll_ex_pid_ctrl.f);
writetable(pll_ex_pid_ctrl, "pll_ex_pid_ctrl.dat", Delimiter='\t');


fig = figure();
ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1]); hold("on");

if SHOW_POLYLINE_COMPRESSION
    plot(f, K_p_mag_resp, 'k'); scatter(pll_ex_p_ctrl.f, pll_ex_p_ctrl.p_ctrl, [], 'k');
    plot(f, K_pi_mag_resp, 'k'); scatter(pll_ex_pi_ctrl.f, pll_ex_pi_ctrl.pi_ctrl, [], 'k');
    plot(f, K_pid_mag_resp, 'k'); scatter(pll_ex_pid_ctrl.f, pll_ex_pid_ctrl.pi_ctrl, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(pll_ex_p_ctrl.f, pll_ex_p_ctrl.p_ctrl, Tag="l:pll_ex:p_ctrl");
plot(pll_ex_pi_ctrl.f, pll_ex_pi_ctrl.pi_ctrl, Tag="l:pll_ex:pi_ctrl");
plot(pll_ex_pid_ctrl.f, pll_ex_pid_ctrl.pi_ctrl, Tag="l:pll_ex:pid_ctrl");
plot(pll_ex_are_ctrl.f, pll_ex_are_ctrl.are_ctrl, "--k", Tag="l:pll_ex:benchmark");

xlabel("Frequency (Hz)");
ylabel("Magnitude (dB)");
%matlab2tikz('showInfo', false, 'extraTikzpictureOptions', 'font=\sffamily', 'parseStrings', false, 'addLabels', true, 'pll_ex_pid_ctrl.pgf');

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_pid_ctrl.pdf", ContentType="vector");

%%
% Figure 6: P gain grid scan
fig = figure();
ax = axes(PlotBoxAspectRatio=[1, 1, 1]); hold("on");

plot(sol_p.DeltaKpGrid, (sol_p.gamma / min(sol_p.gamma(:)) - 1) * 100, Tag="l:pll_ex:p_grid");

xlim("tight");
xticks(-12:3:12);
xtickformat("%+d");
ytickformat("%+d");

xlabel("K_p / K_p^{(opt)} (dB)");
ylabel("H2-norm (%)");

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_p_grid.pdf", ContentType="vector");

%%
% Figure 7: PI gain grid scan
fig = figure();
ax = axes(PlotBoxAspectRatio=[1, 1, 1]); hold("on");

lvls = 0:5:100;
cdata = contour(sol_pi.DeltaKpGrid, sol_pi.DeltaKiGrid, (sol_pi.gamma ./ min(sol_pi.gamma(:)) - 1) * 100, lvls, Tag="l:pll_ex:pi_grid");
lvls2 = 0:1:5;
lvls2 = setdiff(lvls2, lvls);
cdata_fine = contour(sol_pi.DeltaKpGrid, sol_pi.DeltaKiGrid, (sol_pi.gamma ./ min(sol_pi.gamma(:)) - 1) * 100, lvls2, "--");
scatter(0, 0, '+');

writematrix(cdata', "pll_ex_pi_grid.dat", Delimiter='\t');
writematrix(cdata_fine', "pll_ex_pi_grid_fine.dat", Delimiter='\t');

cb = colorbar();
cb.Ruler.TickLabelFormat = "%+g%%";
xtickformat("%+d");
ytickformat("%+d");

xlabel("K_p / K_p^{(opt)} (dB)");
ylabel("K_i / K_i^{(opt)} (dB)");
cb.Label.String = "H2-norm (%)";
%set(ax,'ColorScale','log');

set(gca, LooseInset=max(get(gca, 'TightInset'), 0.02));
exportgraphics(ax, "pll_ex_pi_grid.pdf", ContentType="vector");


%% Helper functions
function unpackStruct(structure)
    fn = fieldnames(structure);
    
    for i = 1:numel(fn)
        fni = string(fn(i));
        field = structure.(fni);
        if (isstruct(field))
            unpackStruct(field);
            continue;
        end
        assignin('base', fni, field);
    end
end