% ### Stabilized Optical Auto-Correlaiton Transmission Line 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 phase distribution.
yalmip('clear'); clear;

%% Transmission Line Model
% System blocks in mrad @ 1.3GHz
drv_bw = 1e3; % 1 kHz driver bandwidth (typical LP setting for PZT4 in link application)
drv_k = 80; % 80 V/V gain (typical PZT4 gain in bipolar mode)
act_k = 0.4; % mrad/V, averaged & rounded from XFEL links (50 fs/V * 2*pi*1.3e-3)
tau = 1000 / (3e8 / 1.47); % 1000m at speed of light in glass fibre
G = zpk([], -2*pi*drv_bw, (2*pi*drv_bw) * drv_k * act_k);

Fr = zpk([], 0, 2);
Fd = tf(0.1, [1, 2*pi*10]);  % Modelled after XFEL Link SLO1
Fn = tf(db2mag(-100));

[P, Pinfo] = build_tl_model(G, Fr, Fd, Fn, tau, Rolloff=10, TimeUnit="milliseconds");
ny = Pinfo.ny; nu = Pinfo.nu;

%% Rational delay approximations
pade_order = 1:5;
length_km = [0, 1, 2, 5, 10];
Ppade = cell(5, 5);

[X, Y] = meshgrid(pade_order, length_km);

for i = 1:numel(X)
    p = P; p.InternalDelay = p.InternalDelay * Y(i);
    Ppade{i} = pade(p, X(i));
    Ppade{i} = minreal(Ppade{i}, 1e-9, false);
    Ppade{i} = balreal(Ppade{i});
    Ppade{i} = truncate_coeff(Ppade{i}, 1e-9);
end; clear i X Y tl_demo_pade_perf_simple;

%%- Toolbox ARE / eps-perturbation method
sol = struct();

h2syn_opts = h2synOptions(AutoScale="on", Regularize="on");
for i = 1:numel(Ppade)
    [sol.K, sol.CL, sol.gamma, sol.info] = h2syn(Ppade{i}, ny, nu, h2syn_opts);
    sol_are(1, i) = sol;
end; clear sol;

sol_are = reshape(sol_are, 1, size(Ppade, 1), size(Ppade, 2));
% fprintf("ARE gamma summary: %s\n", num2str([sol_are(:).gamma]));

K = sol_are(1,1,1).K;
Kpi = zpk(K);
Kpi = Kpi / max(abs(Kpi.P{1}));
%Kpi.P{1} = maxk(Kpi.P{1}, order(Kpi)-1);
Kpi.P{1} = 0;
Kpi = pid(Kpi);
round(Kpi.Kp, 1, "significant");
round(Kpi.Ki, 1, "significant");

%% LMI method
sol = struct();
sdp_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);

h2lmi_opts = h2syn_lmi_options(eliminate_variables=false, sdp_options=sdp_opts, tolerance=1e-9, scale=false);
%h2lmi_opts.regularization_factor = 2;
%h2lmi_opts.relaxation_factor = 10;
for i = 1:numel(Ppade)
    [sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(Ppade{i}, ny, nu, h2lmi_opts);
    sol_lmi(1, i) = sol;
end; clear sol;

h2lmi_opts.tolerance = 0;
h2lmi_opts.maximum_decay = 2 * max(cellfun(@(x) max(abs(real(pole(x)))), Ppade), [], 'all');
for i = 1:numel(Ppade)
    [sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(Ppade{i}, ny, nu, h2lmi_opts);
    sol_lmi(2, i) = sol;
end; clear sol;

h2lmi_opts.minimum_damping = 1/2;
for i = 1:numel(Ppade)
    [sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(Ppade{i}, ny, nu, h2lmi_opts);
    sol_lmi(3, i) = sol;
end; clear sol;

h2lmi_opts.regularization_factor = 2;
h2lmi_opts.relaxation_factor = 2;
for i = 1:numel(Ppade)
    [sol.K, sol.CL, sol.gamma, sol.info] = h2syn_lmi(Ppade{i}, ny, nu, h2lmi_opts);
    sol_lmi(4, i) = sol;
end; clear sol;

sol_lmi = reshape(sol_lmi, size(sol_lmi, 1), size(Ppade, 1), size(Ppade, 2));
%fprintf("LMI gamma summary: %s\n", num2str([sol_lmi(:).gamma]));


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

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);
        for i = 1:numel(Ppade)
            [sol.CL, sol.gamma] = systune(lft(Ppade{i}, ctrl), h2goal, systopt);
            sol.K = ss(sol.CL.Blocks.ctrl);
            sol_ns_tf(end+1) = sol; %#ok<SAGROW>
        end
    end
end; clear sol;
sol_ns_tf = reshape(sol_ns_tf, [], size(Ppade, 1), size(Ppade, 2));
%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);
    for i = 1:numel(Ppade)
        [sol.CL, sol.gamma] = systune(lft(Ppade{i}, ctrl), h2goal, systopt);
        sol.K = ss(sol.CL.Blocks.ctrl);
        sol_ns_ss(end+1) = sol; %#ok<SAGROW> 
    end
end; clear sol;
sol_ns_ss = reshape(sol_ns_ss, [], size(Ppade, 1), size(Ppade, 2));
%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);
    for i = 1:numel(Ppade)
        [sol.CL, sol.gamma] = systune(lft(Ppade{i}, ctrl), h2goal, systopt);
        sol.K = ss(sol.CL.Blocks.ctrl);
        sol_ns_pid(end+1) = sol; %#ok<SAGROW> 
    end
end; clear sol;
sol_ns_pid = reshape(sol_ns_pid, [], size(Ppade, 1), size(Ppade, 2));
%fprintf("Nonsmooth PID tuning gamma summary: %s\n", num2str([sol_ns_pid(:).gamma]));


%%
%- P grid scan
% Kp = logspace(0, 3, 256);
% 
% sol_p = struct('CL', {}, 'gamma', {}, 'K', {}, 'grid', {});
% for i = 1:numel(Kp)
%     K = pid(Kp(i), TimeUnit=P.TimeUnit);
%     CL = lft(P, K);
%     gamma = norm(CL);
%     sol.K = K; sol.CL = CL; sol.gamma = gamma; sol.grid = Kp;
%     sol_p(end+1) = sol; %#ok<SAGROW> 
% end; clear sol K CL gamma;
% 
% fig = figure(); loglog(Kp, [sol_p.gamma]);
% title("P-control, H2-performance vs. controller gain"); xlabel("P-Gain"); ylabel("H2-norm");
% 
% 
% %- PI grid scan
% Kp = linspace(-6, 6, 32);
% Ki = linspace(-6, 6, 32);
% [Kp, Ki] = meshgrid(Kp, Ki);
% 
% sol_pi = struct('CL', {}, 'gamma', {}, 'K', {}, 'grid', {});
% for i = 1:numel(Kp)
%     K = pid(sol_ns_pid(2).K.Kp * db2mag(Kp(i)), sol_ns_pid(2).K.Ki * db2mag(Ki(i)), TimeUnit=P.TimeUnit);
%     CL = lft(P, K);
%     gamma = norm(CL);
%     sol.K = K; sol.CL = CL; sol.gamma = gamma; sol.grid = {Kp, Ki};
%     sol_pi(end+1) = sol; %#ok<SAGROW> 
% end; clear sol K CL gamma;
% 
% figure; contour(Kp, Ki, reshape(1e3 * [sol_pi.gamma] * sqrt(1e3), size(Kp)));
% title("PI-control, H2-performance vs. controller gain"); xlabel("P-Gain (dB_{opt})"); ylabel("I-Gain (db_{opt})"); colorbar();
% 
% %- PD grid scan
% Kp = logspace(0, 4, 32);
% Kd = logspace(-2, 4, 32);
% [Kp, Kd] = meshgrid(Kp, Kd);
% 
% sol_pd = struct('CL', {}, 'gamma', {}, 'K', {}, 'grid', {});
% for i = 1:numel(Kp)
%     K = pid(Kp(i), 0, Kd(i), 4.5, TimeUnit=P.TimeUnit);
%     CL = lft(P, K);
%     gamma = norm(CL);
%     sol.K = K; sol.CL = CL; sol.gamma = gamma; sol.grid = {Kp, Kd};
%     sol_pd(end+1) = sol; %#ok<SAGROW> 
% end; clear sol K CL gamma;
% 
% fig = figure(); contour(Kp, -Kd, reshape(log10([sol_pd.gamma]), size(Kp)), 20); ax = gca; ax.XScale = 'log'; ax.YScale = 'log';
% title("PD-control, H2-performance vs. controller gain"); xlabel("P-Gain"); ylabel("D-gain");


%% Robust case
tau_unc = ureal("tau", 0);
dly_ph = tf(tau_unc, 1, TimeUnit="milliseconds");

% Ignore disturbance inputs for stability analysis
lfr.M = [0, G;
         -1, -G];
lfr.M = chgTimeUnit(lfr.M, "milliseconds");
lfr.M = lft(lfr.M, Kpi);
lfr.Delta = tau_unc;
lfr.BlkStruct = struct(Name='tau', Size=[1 1], Type='ureal', Occurrences=1, Simplify=2);


% static delay multiplier - for infinite delay margin
Pi1 = [0, -1;
       -1, -1];
tau_unc.NominalValue = 1e-3; tau_unc.Range=[0,2];
phi2 = 2 * tf([tau_unc*tau_unc, 3.5*tau_unc, 1e-6], [tau_unc*tau_unc, 4.5*tau_unc, 7.1]);
phi2 = usubs(phi2, "tau", 2*49e-6);
phi2 = chgTimeUnit(phi2, "milliseconds");
Pi2 = blkdiag(phi2*phi2', -1);
phi3 = tf([-2.19*tau_unc*tau_unc, 9.02*tau_unc, 89e-3], [tau_unc*tau_unc, -5.64*tau_unc, -17]);
phi3 = usubs(phi3, "tau", 2*49e-6);
phi3 = chgTimeUnit(phi3, "milliseconds");
Pi3 = [0, phi3';
       phi3, -1];

% Before trying KYP lemma, verify time delay margin using frequency gridding
Mqp = feedback(lfr.M, -1); % loop shift
iqc_fdi_grid(Mqp, Pi1, [], 0, 0);
iqc_fdi_grid(Mqp, Pi2, [], 0, 0);
iqc_fdi_grid(Mqp, Pi3, [], 0, 0);

% KYP Lemma
sdp_opts = sdpsettings('solver', 'mosek', 'debug', 1, 'verbose', 2);

n = order(Mqp);
nq = size(lfr.Delta, 1);
tmp = ss([Mqp; nq]);

yalmip('clear');
X = sdpvar(n);

cnst = [[eye(n), zeros(n, 1);
         tmp.A, tmp.B;
         tmp.C, tmp.D]' * blkdiag(fOblkdiag(X), Pi1) * [eye(n), zeros(n, 1);
                                                        tmp.A, tmp.B;
                                                        tmp.C, tmp.D] <= -1e-9 * eye(n+nq)];
sol_diag = optimize(cnst, [], sdp_opts);

%% With Performance
yalmip('clear');
lfr = Pinfo.lfr;
lfr.M = lft(lfr.M, Kpi);
lfr.M = minreal(ss(lfr.M));

np = size(lfr.Delta, 1);
nq = size(lfr.Delta, 2);
nz = size(lfr.M, 1) - np;
nw = size(lfr.M, 2) - nq;
n = order(lfr.M);

loop_shift = kron([0, 1; 1 1], eye(nq));
lfr.M = ss(lft(loop_shift, lfr.M, 2, 2));

% static delay multiplier - for infinite delay margin
Pi1 = kron([0, -1; -1, -1], eye(nq));
Theta1 = Pi1;
Psi1 = eye(size(Pi1, 1));

phi2 = 2 * tf([tau_unc*tau_unc, 3.5*tau_unc, 1e-6], [tau_unc*tau_unc, 4.5*tau_unc, 7.1]);
phi2 = usubs(phi2, "tau", 49e-6);
phi2 = chgTimeUnit(phi2, "milliseconds");
Pi2 = blkdiag(blkdiag(phi2*phi2', phi2*phi2'), diag([-1, -1]));
[Psi2, Theta2] = iqc_factorization(Pi2);
phi3 = tf([-2.19*tau_unc*tau_unc, 9.02*tau_unc, 89e-3], [tau_unc*tau_unc, -5.64*tau_unc, -17]);
phi3 = usubs(phi3, "tau", 49e-6);
phi3 = chgTimeUnit(phi3, "milliseconds");
Pi3 = [zeros(2), blkdiag(phi3', phi3');
       blkdiag(phi3, phi3), -1 * eye(2)];
[Psi3, Theta3] = iqc_factorization(Pi3);

L = 3;
rho = ureal("rho", 1);
base = tf(1, [1 rho]);
PsiN = [1; base; base^2];
PsiN_ = usubs(PsiN, "rho", 1);
epsilon = ureal("epsilon", 1e-2);
PsiTd = 2 * tf([tau_unc*tau_unc, tau_unc*(4/pi+epsilon), 4/pi*epsilon], [tau_unc*tau_unc, -pi*cos(pi^2/4)*tau_unc, pi^2/4]);
PsiTd_ = usubs(PsiTd, "tau", 49e-6, "epsilon", 1e-3);
PsiX = [fsskron(ss(PsiN_*PsiTd_), nq), zeros(nq*size(PsiN, 1), nq);
        fsskron(ss(PsiN_), nq), zeros(nq*size(PsiN, 1), nq);
        zeros(nq*size(PsiN,1), nq), fsskron(ss(PsiN_), nq)];
PsiX = chgTimeUnit(PsiX, "milliseconds");
ThetaXp1 = sdpvar(L*nq);
ThetaXp2 = sdpvar(L*nq);
ThetaX = [ThetaXp1, zeros(L*nq), zeros(L*nq);
          zeros(L*nq), zeros(L*nq), ThetaXp2;
          zeros(L*nq), ThetaXp2, ThetaXp2 - ThetaXp1];

%iqc_fdi_grid(minreal(lfr.M(1:2,1:2)), Pi1, [], 0, 0);
%iqc_fdi_grid(minreal(lfr.M(1:2,1:2)), Pi2, [], 0, 0);
%iqc_fdi_grid(minreal(lfr.M(1:2,1:2)), Pi3, [], 0, 0);
%iqc_fdi_grid(lfr.M, Pi1, blkdiag(eye(nz), zeros(nw)), 3, 1);
%iqc_fdi_grid(lfr.M, Pi2, blkdiag(eye(nz), zeros(nw)), 3, 1);
%iqc_fdi_grid(lfr.M, Pi3, blkdiag(eye(nz), zeros(nw)), 3, 1);

tmp = minreal(blkdiag(PsiX, eye(nz)) * [lfr.M(1:nq, :); eye(nq), zeros(nq, nw); lfr.M(nq+1:end, :)]);
tmp = balreal(tmp);
n = order(tmp);

Bz = tmp.B(:, np+1:end);
tmp = tmp(:, 1:np);


X = sdpvar(n);
gamma_sq = sdpvar();
%p = sdpvar(1,3);

cnst = [trace(Bz' * X * Bz) <= gamma_sq;
        %p >= 1;
        [eye(n), zeros(n, np);
         tmp.A, tmp.B;
         tmp.C, tmp.D]' * blkdiag(fOblkdiag(X), ThetaX, eye(nz)) * [eye(n), zeros(n, np);
                                                                    tmp.A, tmp.B;
                                                                    tmp.C, tmp.D] <= -1e-9 * eye(n+nq)];

sol_diag = optimize(cnst, gamma_sq, sdp_opts);

PiX = minreal(PsiX' * value(ThetaX) * PsiX);
iqc_fdi_grid(minreal(lfr.M(1:2,1:2)), PiX, [], 0, 0);

%% Scan over rho
rho = logspace(-1, 3, 16);
gamma = zeros(length(rho), 1);
vio = zeros(length(rho), 1);
for i = 1:length(rho)
    PsiN_ = usubs(PsiN, "rho", rho(i));
    PsiX = [fsskron(ss(PsiN_*PsiTd_), nq), zeros(nq*size(PsiN_, 1), nq);
            fsskron(ss(PsiN_), nq), zeros(nq*size(PsiN_, 1), nq);
            zeros(nq*size(PsiN_, 1), nq), fsskron(ss(PsiN_), nq)];
    PsiX = chgTimeUnit(PsiX, "milliseconds");
    
    tmp = minreal(blkdiag(PsiX, eye(nz)) * [lfr.M(1:nq, :); eye(nq), zeros(nq, nw); lfr.M(nq+1:end, :)]);
    tmp = balreal(tmp);
    n = order(tmp);
    X = sdpvar(n);
    
    Bz = tmp.B(:, np+1:end);
    tmp = tmp(:, 1:np);
    
    cnst = [trace(Bz' * X * Bz) <= gamma_sq;
            [eye(n), zeros(n, np);
             tmp.A, tmp.B;
             tmp.C, tmp.D]' * blkdiag(fOblkdiag(X), ThetaX, eye(nz)) * [eye(n), zeros(n, np);
                                                                        tmp.A, tmp.B;
                                                                        tmp.C, tmp.D] <= -1e-9 * eye(n+nq)];
    
    sol_diag = optimize(cnst, gamma_sq, sdp_opts);
    gamma(i) = sqrt(value(gamma_sq));
    PiX = minreal(PsiX' * value(ThetaX) * PsiX);
    iqc_fdi_grid(minreal(lfr.M(1:2,1:2)), PiX, [], 0, 0);
end

%% Scan over tau
gamma = zeros(10, 1);
for i = 1:length(gamma)
    PsiTd_ = usubs(PsiTd, "tau", i*4.9e-6, "epsilon", 1e-3);
    PsiX = [fsskron(ss(PsiN*PsiTd_), nq), zeros(nq*size(PsiN, 1), nq);
            fsskron(ss(PsiN), nq), zeros(nq*size(PsiN, 1), nq);
            zeros(nq*size(PsiN,1), nq), fsskron(ss(PsiN), nq)];
    PsiX = chgTimeUnit(PsiX, "milliseconds");
    
    tmp = minreal(blkdiag(PsiX, eye(nz)) * [lfr.M(1:nq, :); eye(nq), zeros(nq, nw); lfr.M(nq+1:end, :)]);
    tmp = balreal(tmp);
    n = order(tmp);
    X = sdpvar(n);
    
    Bz = tmp.B(:, np+1:end);
    tmp = tmp(:, 1:np);
    
    cnst = [gamma_sq >= 1e-9;
            trace(Bz' * X * Bz) <= gamma_sq;
            [eye(n), zeros(n, np);
             tmp.A, tmp.B;
             tmp.C, tmp.D]' * blkdiag(fOblkdiag(X), ThetaX, eye(nz)) * [eye(n), zeros(n, np);
                                                                        tmp.A, tmp.B;
                                                                        tmp.C, tmp.D] <= -1e-9 * eye(n+nq)];
    
    sol_diag = optimize(cnst, gamma_sq, sdp_opts);
    check(cnst);
    gamma(i) = sqrt(value(gamma_sq));
end
%figure();
plot(gamma);


%% Try with IQC toolbox
dlt = iqcdelta('delta', InputChannel=1:2, OutputChannel=1:2, StaticDynamic='D', DelayType=1, DelayTime=49e-3);
pe = iqcdelta('h2', ChannelClass='P', InputChannels=3:5, OutputChannels=3, PerfMetric='H2');
udlt = iqcassign(dlt, 'udel', Length=2, AddIQC='yes');

iqclabans = iqcanalysis(lfr.M, {udlt,pe}, Parser="Yalmip", SolChk='on', Solver='mosek');

return;

%% 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"))));
tl_ex_g = array2table(reducepoly([f_lg, G_mag_resp], 5e-5), VariableNames=["f", "g"]);
tl_ex_g.f = 10.^(tl_ex_g.f);
writetable(tl_ex_g, "tl_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(tl_ex_g.f, tl_ex_g.g, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(tl_ex_g.f, tl_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"))));

tl_ex_fr = array2table(reducepoly([f_lg, Fr_mag_resp], 5e-5), VariableNames=["f", "fr"]);
tl_ex_fr.f = 10.^(tl_ex_fr.f);
writetable(tl_ex_fr, "tl_ex_fr.dat", Delimiter='\t');
tl_ex_fd = array2table(reducepoly([f_lg, Fd_mag_resp], 5e-5), VariableNames=["f", "fd"]);
tl_ex_fd.f = 10.^(tl_ex_fd.f);
writetable(tl_ex_fd, "tl_ex_fd.dat", Delimiter='\t');
tl_ex_fn = array2table(reducepoly([f_lg, Fn_mag_resp], 1), VariableNames=["f", "fn"]);
tl_ex_fn.f = 10.^(tl_ex_fn.f);
writetable(tl_ex_fn, "tl_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(tl_ex_fr.f, tl_ex_fr.fr, [], 'k');
    plot(f, Fd_mag_resp, 'k'); scatter(tl_ex_fd.f, tl_ex_fd.fd, [], 'k');
    plot(f, Fn_mag_resp, 'k'); scatter(tl_ex_fn.f, tl_ex_fn.fn, [], 'k');
    ax.ColorOrderIndex = 1;
end

plot(tl_ex_fr.f, tl_ex_fr.fr, Tag="l:pll_ex:fr");
plot(tl_ex_fd.f, tl_ex_fd.fd, Tag="l:pll_ex:fd");
plot(tl_ex_fn.f, tl_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 1: Pade performance
tl_demo_pade_perf_simple = table(length_km', 1e3 * cellfun(@(x) norm(lft(x, sol_are(1,1,1).K)), Ppade(:,4)), VariableNames=["l", "gamma"]);
writetable(tl_demo_pade_perf_simple, "tl_demo_pade_perf_simple.dat", Delimiter="\t");
tl_demo_pade_perf_optimal = table(length_km', 1e3 * arrayfun(@(x) x.gamma, sol_are(1,:,4))', VariableNames=["l", "gamma"]);
writetable(tl_demo_pade_perf_optimal, "tl_demo_pade_perf_optimal.dat", Delimiter="\t");

% TODO: second x-axis: time delay, second y-axis integrated jitter fs
fig = figure(Units="centimeters");
fig.Position(3:4) = [7, 7];

ax = axes(PlotBoxAspectRatio=[1, 1, 1], XMinorGrid="off", YMinorGrid="off", FontSize=10);
hold("on");

plot(ax, tl_demo_pade_perf_simple.l, tl_demo_pade_perf_simple.gamma, '-o', Tag="l:tl_demo_pade_perf:simple");
plot(ax, tl_demo_pade_perf_optimal.l, tl_demo_pade_perf_optimal.gamma, '-*', Tag="l:tl_demo:optimal");

xlabel("Transmission Line Length (km)");
ylabel("H2-Norm (urad)");
%matlab2tikz('showInfo', false, 'parseStrings', false, 'addLabels', true, ...
%            'extraAxisOptions', 'tl_demo_pade_perf.pgf');

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


%% Figure 2: higher order controller
f = logspace(0, 6, 2^10)';
f_lg = linspace(0, 6, 2^10)';

Kpi_mag_resp = mag2db(abs(squeeze(freqresp(Kpi, f, "Hz"))));
Kare_mag_resp = mag2db(abs(squeeze(freqresp(sol_are(1,end,4).K, f, "Hz"))));

tl_demo_pade_ctrl_pi = array2table(reducepoly([f_lg, Kpi_mag_resp], 5e-5), VariableNames=["f", "pi_ctrl"]);
tl_demo_pade_ctrl_pi.f = 10.^(tl_demo_pade_ctrl_pi.f);
writetable(tl_demo_pade_ctrl_pi, "tl_demo_pade_ctrl_pi.dat", Delimiter='\t');
tl_demo_pade_ctrl_full = array2table(reducepoly([f_lg, Kare_mag_resp], 5e-5), VariableNames=["f", "are_ctrl"]);
tl_demo_pade_ctrl_full.f = 10.^(tl_demo_pade_ctrl_full.f);
writetable(tl_demo_pade_ctrl_full, "tl_demo_ctrl_full.dat", Delimiter='\t');

fig = figure(); ax = axes(XScale="log"); hold("on"); % verify polyline compression
plot(f, Kpi_mag_resp);
plot(tl_demo_pade_ctrl_pi.f, tl_demo_pade_ctrl_pi.pi_ctrl, Color=clrs(1,:)); scatter(tl_demo_pade_ctrl_pi.f, tl_demo_pade_ctrl_pi.pi_ctrl, [], clrs(1,:));
plot(f, Kare_mag_resp);
plot(tl_demo_pade_ctrl_full.f, tl_demo_pade_ctrl_full.are_ctrl, Color=clrs(2,:)); scatter(tl_demo_pade_ctrl_full.f, tl_demo_pade_ctrl_full.are_ctrl, [], clrs(2,:));

fig = figure(Units="centimeters"); % Actual plot
fig.Position(3:4) = [7, 7];

ax = axes(XScale="log", PlotBoxAspectRatio=[1, 1, 1], XMinorGrid="off", YMinorGrid="off", FontSize=10);
hold("on");

plot(f, Kpi_mag_resp, Tag="l:tl_demo_pade_ctrl:pi");
plot(f, Kare_mag_resp, Tag="l:tl_demo_pade_ctrl:full");
ylim([-60, 0]);

xlabel("Frequency (Hz)");
ylabel("Magnitude (dB)");
%matlab2tikz('showInfo', false, 'parseStrings', false, 'addLabels', true, ...
%            'extraAxisOptions', 'tl_demo_pade_ctrl.pgf');

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

%% Figure 3: Time Delay Root Locus
Kpi2 = Kpi;
Kpi2.Ki = Kpi.Ki * 10;
l = logspace(0,2,2^12);
pls = [];
pls2 = [];
for i = 1:numel(l)
    p = P; p.InternalDelay = p.InternalDelay * l(i);
    p = pade(p,4);
    cl = lft(p, Kpi);
    cl2 = lft(p, Kpi2);
    pls(:,:,i) = cl.A;
    pls2(:,:,i) = cl2.A;
end; clear p cl;
[~,pls] = eigenshuffle(pls);
[~,pls2] = eigenshuffle(pls2);


fig = figure(Units="centimeters");
fig.Position(3:4) = [7, 7];

ax = axes(PlotBoxAspectRatio=[1, 1, 1], XMinorGrid="off", YMinorGrid="off", FontSize=10);
hold("on");
clrs = colororder();
for i = 1:size(pls,1)
    z = unique(pls(i,:), "stable");
    if length(z) == 1
        scatter(real(z), imag(z), 'x', clrs(1,:), [], Tag="l:tl_pade_dly_rlocus:stable"+i)
    else
        [x, y] = compress_curve(real(z), imag(z), 1e-2);
        fprintf("Using " + length(x) + " points.\n");
        plot(x, y, color=clrs(1,:), Tag="l:tl_pade_dly_rlocus:stable"+i);
    end
end
for i = 1:size(pls2,1)
    z = unique(pls2(i,:), "stable");
    if length(z) == 1
        scatter(real(z), imag(z), [], clrs(2,:), 'x', Tag="l:tl_pade_dly_rlocus:unstable"+i)
    else
        [x, y] = compress_curve(real(z), imag(z), 1e-2);
        fprintf("Using " + length(x) + " points.\n");
        plot(x, y, color=clrs(2,:), Tag="l:tl_pade_dly_rlocus:unstable"+i);
    end
end
xlim([-100, 10]);
ylim([-100, 100]);

xlabel("Real Part", Interpreter="none");
ylabel("Imaginary Part", Interpreter="none");
%matlab2tikz('showInfo', false, 'parseStrings', false, 'addLabels', true, ...
%            'extraAxisOptions', 'tl_pade_dly_rlocus.pgf');

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

%% Figure 4: Robust h2 vs pade h2
tl_demo_rob_perf = table([1:10]', 1e3 * gamma, VariableNames=["l", "gamma"]);
writetable(tl_demo_rob_perf, "tl_demo_rob_perf.dat", Delimiter="\t");

% TODO: second x-axis: time delay, second y-axis integrated jitter fs
fig = figure(Units="centimeters");
fig.Position(3:4) = [7, 7];

ax = axes(PlotBoxAspectRatio=[1, 1, 1], XMinorGrid="off", YMinorGrid="off", FontSize=10);
hold("on");
    
plot(ax, tl_demo_rob_perf.l, tl_demo_rob_perf.gamma, '-*', Tag="l:tl_demo_rob_perf:iqc");
plot(ax, tl_demo_pade_perf_simple.l, tl_demo_pade_perf_simple.gamma, '--o', Tag="l:tl_demo_rob_perf:pade");

xlabel("Transmission Line Length (km)");
ylabel("H2-Norm (urad)");
%matlab2tikz('showInfo', false, 'parseStrings', false, 'addLabels', true, ...
%            'extraAxisOptions', 'tl_demo_rob_perf.pgf');

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