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

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

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{:});


%%
%- Build model
time_unit = "milliseconds";
scaling = 15;

f = lbsync.xfel.mo.Fd.UserData.raw.data{1}.Frequency';
f = f(10 <= f & f <= 1e6);
w = 2*pi * f * 1e-3;

mo = chgTimeUnit(10^(scaling - lbsync.xfel.mo.scaling) * lbsync.xfel.mo.Fd, time_unit);
mo.P{1}(2) = 1e-3 * 2*pi * -1;
mo.InputName = "w"; mo.Outputname = "y";
mo.OutputUnit = "femtoseconds";

mlo = build_laser_model(lbsync.xfel.laser.mlo1, Scaling=15, TimeUnit=time_unit, FilterMinDecay=1e0);
slo = build_laser_model(lbsync.xfel.laser.slo1, Scaling=15, TimeUnit=time_unit, FilterMinDecay=1e0);
slolink = build_link_model(lbsync.xfel.link.slo1, Scaling=15, TimeUnit=time_unit, FilterMinDecay=1e0);
relonk = build_link_model(lbsync.xfel.link.relonk, Scaling=15, TimeUnit=time_unit, FilterMinDecay=1e0);

ctrl_rate = 1.3e9 / 48 / 16;
mlo_p = realp("mlo_p", 0);
mlo_i = realp("mlo_i", 0);
slo_p = realp("slo_p", 0);
slo_i = realp("slo_i", 0);
lonk_p = realp("lonk_p", 0);
lonk_i = realp("lonk_i", 0);
relonk_p = realp("relonk_p", 0);
relonk_i = realp("relonk_i", 0);

%%
CL_rtrip = chain2(mo, mlo.Pcol, slolink.Pcol, slo.Pcol, relonk.Pcol, AnalysisPoint={'r(2)'});
CL_rtrip = lft(CL_rtrip, blkdiag(tf([mlo_p, mlo_i * ctrl_rate / 1e3], [1, 0], TimeUnit="milliseconds") / mlo.k_det / 2.2 / mlo.input_scaling, tf([lonk_p, lonk_i * 1e3], [1, 0], TimeUnit="milliseconds") / slolink.k_det, ...
                                 tf([slo_p, slo_i * ctrl_rate / 1e3], [1, 0], TimeUnit="milliseconds") / slo.k_det / 2.2 / slo.input_scaling, tf([relonk_p, relonk_i * 1e3], [1, 0], TimeUnit="milliseconds") / relonk.k_det));
CL_rtrip_e = getIOTransfer(CL_rtrip, 'w', {'r(2)', 'y'});
CL_rtrip_e = [1, -1] * replaceBlock(CL_rtrip_e, "CONNECT_AP1", 1);
CL_rtrip_e.OutputName = "phi"; CL_rtrip_e.OutputUnit = "fs";

[H, B, S] = getLFTModel(CL_rtrip_e);
H_pade = pade(H, 2);

B = diag([mlo.C.Kp, mlo.C.Ki / ctrl_rate / 3 * 1e3, slolink.C.Kp, slolink.C.Ki / 1e3, ...
         slo.C.Kp, slo.C.Ki / ctrl_rate / 3 * 1e3, relonk.C.Kp, relonk.C.Ki / 1e3]);