clear; close all;

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

sys = build_model(lbsync.lab);

G = sys.G;
C = pid(10, 0.1);

Fr = sys.Fr;
%F_r = chgTimeUnit(sys.F_r, 'milliseconds');
Fr.P{1}(1) = 0; % Add integral behaviour
Fr = ss(Fr);
%Fr.D = 0; % Explicitly strip feed-through for H2-Norm computation

Fd = sys.Fd;
%F_d = chgTimeUnit(sys.F_d, 'milliseconds');
Fd.P{1}(1) = 0; % Add integral behaviour
Fd = ss(Fd);
%Fd.D = 0; % Explicitly strip feed-through for H2-Norm computation

f = logspace(0, 6, 1e4);


Hr_pwr = squeeze(abs(freqresp(Fr, f, 'Hz')).^2);
Hd_pwr = squeeze(abs(freqresp(Fd, f, 'Hz')).^2);

opts = optimoptions('fmincon', 'Display', 'off');


sys.pzt_lp_cfreq = inf;

% [24e3, 30e3, 35e3, 40e3, 45e3, 50e3]
for cf = [0.3e3, 1e3, 10e3, 30e3, 50e3, 80e3, 100e3, 150e3, inf]
    %sys.pzt_fixed_pole_cfreq(1) = cf;
    sys.pzt_lp_cfreq = cf;
    sys = build_model(sys);
    fun = @(k) get_total_jitter(f, sys, Hr_pwr, Hd_pwr, k(1), k(2));
    [opt_gains, jitter] = fmincon(fun, [1, 1], [], [], [], [], [0, 0], [5e2, 1e5], [], opts);
    %fprintf('Optimal for CF=%g Hz: Kp = %.3f, Ki = %.3f, J = %.3f\n', cf, opt_gains(1), opt_gains(2), jitter);
    fprintf('%g\t%.3f\t%.3f\t%.3f\n', cf, opt_gains(1), opt_gains(2), jitter);
end; clear cf

function total_jitter = get_total_jitter(f, sys, Hr_pwr, Hd_pwr, p_gain, i_gain)
    C = pid(p_gain, i_gain, 0, 0) / sys.k_phi;
    S = 1 / (C * sys.G + 1);
    %T = C * sys.G * S;
    S_fr = squeeze(abs(freqresp(S, f, 'Hz')).^2);
    %T_fr = squeeze(abs(freqresp(T, f, 'Hz')).^2);
    y = S_fr .* (Hr_pwr + Hd_pwr);
    total_jitter = sqrt(trapz(f, y));
end