% Documentation how to build a tunable / parameterized link-lock model from scratch.
% - Time unit 'milliseconds' is chosen to yield good coefficients for frequency range of interest ~10Hz - 1MHz.
% !! Things can easily go wrong when initializing model with final time unit. Hence modeling in seconds, then convert.
% - Input/output units are scaled respectively with the same idea.
% - UserData is used as struct to store parameters.
% - Notes is used as description in bullet point list format.

% Convenience stuff
c = 3e8;  % speed of light
neff = 1.47;  % effective refractive index
ms = 'milliseconds';
fs = 'femtoseconds';
tu = ms;  % Target time unit
s = chgTimeUnit(tf('s'), tu);

% Analysis points
ap_u = AnalysisPoint('u');
ap_u.Name = 'ap_input';
ap_u.Location{1} = 'Control action';
ap_u = chgTimeUnit(ap_u, 'milliseconds');
ap_u.InputName = 'Control action'; ap_u.OutputName = ap_u.InputName;
ap_e = AnalysisPoint('e');
ap_e.Name = 'ap_error';
ap_e.Location{1} = 'Measured timing error';
ap_e = chgTimeUnit(ap_e, 'milliseconds');
ap_e.InputName = 'Measured timing error'; ap_e.InputUnit = 'femtosecond';
ap_e.OutputName = ap_e.InputName; ap_e.OutputUnit = 'femtosecond';
ap_r = AnalysisPoint('r');
ap_r.Name = 'ap_reference';
ap_r.Location{1} = 'Reference timing';
ap_r = chgTimeUnit(ap_r, tu);
ap_r.InputName = 'Reference timing';
ap_r.OutputName = 'Reference timing';

% Piezo fiber stretcher dynamics
pz_c = 92e-9;  % 92nF typical for Optiphase PZ2
pz_imp = tf(1, [pz_c, 0], ...
            OutputUnit='ohm');  % Base model of ideal piezo impedance (pure capacitive, 92nF): 1/(92e-9 * s)
pz_imp = chgTimeUnit(pz_imp, tu);  % Change to target timeunit;
pz_imp = tunableTF('pz_imp', pz_imp);  % Make this a tunableTF object so it can be replaced easily in final model.
pz_imp.Numerator.Free = 0; pz_imp.Denominator.Free = 0;  % This is fixed, not meant to be optimized by a tuning algo.
pz_imp.UserData = struct(capacitance=pz_c);
pz_imp.Notes = ["Ideal piezo crystal with pure capacitive response.";
                "92nF typical for Optiphase PZ2 fiber stretcher."];

% By Ohm's law, the impedance enters the model equation reciprocal, i.e. improper. Toolbox doesn't handle this well, so
% we are doing the cancellation with the first model integrator (integrating current to charge (prop. elongation)) explcitily here.
% Q = \int U / Z dt

pz_chrg = tf(pz_imp.UserData.capacitance, 1, ...   % note that this is inverted from before!
             InputUnit = 'volt', ...
             OutputUnit = 'coulomb');
pz_chrg = chgTimeUnit(pz_chrg, tu);
pz_chrg = tunableTF('pz_chrg', pz_chrg);
pz_chrg.Numerator.Free = 0; pz_chrg.Denominator.Free = 0;
pz_chrg.UserData = struct(capacitance=pz_imp.UserData.capacitance, ...
                          base_model=pz_imp);  % Store the impedance measurement result as reference.
pz_chrg.Notes = [pz_imp.Notes;
                "Charge accumulation model adapted from impedance model."];


% Link
path_dly_gain = realp('path_dly_gain', 46);  % Induced delay per V for all 4 passes (fs/V).
path_dly_gain.Free = 0;
link = path_dly_gain * pz_chrg / dcgain(pz_chrg);
link.InputName = "Tuning voltage"; link.InputUnit = 'volt';
link.OutputName = "Path delay"; link.OutputUnit = 'femtosecond';
link.Notes = ["";
              ""];

link_len = realp('link_len', 4000);  % Link length, rounded from XFEL LONK
link_len.Free = 0; link_len.Minimum = 0;
link_vel_fctr = realp('link_vel_fctr', 1/neff);
link_vel_fctr.Free = 0;
link_dly = exp(-s * link_len.Value / c / link_vel_fctr.Value);  % Parameterized delays not supported


% Driver (PZT4)
drv_gain = realp('drv_gain', 80);  % Gain of the PZT4 piezo driver in unipolar mode.
drv_gain.Free = 0;
drv_bw = realp('drv_bandwidth', 2.4394e+04);  % Bandwidth of the PZT4 piezo amplifier power amplifier stage.
drv_bw.Free = 0;
drv_lp_cfreq = realp('drv_lp_cfreq', 1e3);  % Corner frequency of the PZT4 configurable low-pass filter.
drv_lp_cfreq.Free = 0;

drv = drv_gain * tf(1, [1/(2*pi*drv_lp_cfreq), 1]) * tf(1, [1/(2*pi*drv_bw), 1]);
drv = chgTimeUnit(drv, tu);
drv.InputName = "Input voltage"; drv.InputUnit = 'volt';
drv.OutputName = "Output voltage"; drv.OutputUnit = 'volt';
drv.Notes = ["Model of a MTCA.4 PZT4 piezo amplifier board channel in unipolar mode.";
             "`drv_gain` is the DC gain of the driver";
             "`driver_bandwidth` is the hard bandwidth of the driver's amplifier circuit.";
             "`driver_lp_cfreq` is the user configurable corner frequency of the first-order output low-pass filter."];

% Optional: reference filter
ref_fltr = zpk([-481.1, -7115], [-114.4, -2019, -2142], 8.9635 * 118.6, ...
               InputName="White reference noise", ...
               OutputName="Reference timing", ...
               OutputUnit="femtosecond");
ref_fltr = chgTimeUnit(ref_fltr, 'milliseconds');
ref_fltr = tunableTF('ref_fltr', ref_fltr);
ref_fltr.Numerator.Free = 0;
ref_fltr.Denominator.Free = 0;
ref_fltr.Notes = "Adapted from MO phase noise identified in M. Heuer thesis with removed (measurement) noise floor";

% Output Disturbance
dtrb_fltr = tf([-0.3790, 0.2756], [1, 26.8028, 0], ...
                InputName="White disturbance noise", ...
                OutputName="Time disturbance", ...
                OutputUnit='femtosecond', ...
                TimeUnit='milliseconds');
dtrb_fltr = tunableTF('dtrb_fltr', tf(dtrb_fltr));
dtrb_fltr.Numerator.Free = 0;
dtrb_fltr.Denominator.Free = 0;
dtrb_fltr.Notes = "Adapted from LONK1 measurement.";


% Measurement Noise
meas_noise_fltr = tf(db2mag(-70), 1, ...
                     InputName="White measurement noise", ...
                     OutputName="Measurement noise", ...
                     OutputUnit='femtosecond');
meas_noise_fltr = tunableTF('meas_noise_fltr', meas_noise_fltr);
meas_noise_fltr.Numerator.Free = 0;
meas_noise_fltr.Denominator.Free = 0;
meas_noise_fltr = chgTimeUnit(meas_noise_fltr, tu);
meas_noise_fltr.Notes = ["Link detection chain noise coloring filter.";
                         "Averaged from various BPD measurements."];


% Controller
ctrl = pid(3e-5, 374e-6, ...
           InputName="Measured time error", ...
           InputUnit='femtosecond', ...
           OutputName="Control action");
ctrl = tunablePID('ctrl', ctrl);
ctrl = chgTimeUnit(ctrl, tu);
ctrl.UserData = struct(update_rate=1e6);
ctrl.Notes = ["Scaled, continuous-time approximation of LbSync link-lock firmware PID controller.";
              "Output range is [-1,1] volts";
              "Controller rate calculated as 5e6 / 5 from input downsampling."];


% Connect open-loop plant
G = link * drv;
G.Notes = [link.Notes;
           drv.Notes];

% Connect generalized plant according to block diagram
P = connect(append(G/2, link_dly, link_dly, ap_r * ref_fltr, dtrb_fltr/2, meas_noise_fltr, ...
                   tf(1, OutputName="Disturbed timing", OutputUnit='femtosecond'), ...
                   tf(1, OutputName="Measured timing error", OutputUnit='femtosecond')), ...
            [2, 1, 4, 0, 0; ...
             7, 2, 5, 0, 0; ...
             3, 5, 7, 0, 0; ...
             8, -1, -3, 4, -6], ...
            [4, 5, 6, 1], ...
            [7, 8]);
P.InputName = ["Reference timing", "White disturbance noise", "White measurement noise", "Control action"]; P.InputUnit = ["femtosecond", "", "", ""];
% P.OutputName = ["Disturbed timing", "Measured timing error"]; P.OutputUnit = {'femtosecond', 'femtosecond'};


% Connect closed loop plant
CL = lft(P, ap_u * ctrl * ap_e);
CL.Notes = [P.Notes; ctrl.Notes];


% Get model
CLss = prescale(ss(CL));  % prescale
%CLss = minreal(CLss);  % be careful that all cancellations are valid!
%CLss = truncate_coeff(CLss, 1e-9);  % get rid of numerical noise

% Tuning goal
h2goal = TuningGoal.Variance(CL.InputName, CL.OutputName, 1);
CLopt = systune(CL, h2goal);

ssCLopt = truncate_coeff(prescale(ss(CLopt)), 1e-9);



% Connected generalized plant according to block diagram
% P = connect(append(ap_u * ctrl * ap_e, G/2, link_dly, link_dly, dtrb_fltr/2, meas_noise_fltr, 1, 1), ...
%             [1, -2, -4, 6;
%              2, 1, 0, 0;
%              3, 2, 0, 0;
%              7, 3, 5, 0;
%              4, 5, 7, 0;
%              8, -7, 0, 0], ...
%             [1, 3, 8;
%              5, 0, 0;
%              6, 0, 0], ...
%             [7, 8]);
% 
% 
% Connect generalized plant
% P = blkdiag(1, 1, link_dly, 1) * parallel(tf(ones(3, 1), 1, InputName="Reference timing", InputUnit='femtosecond'), ones(2, 1) * G/2, [], [], 2, 1);  % Add reference timing to forward-path delay (half of round-trip path delay).
% P.OutputName = ["Reference timing", "Reference timing", "Timing", "Path delay"]; P.OutputUnit = 'femtosecond';
% P = parallel(ones(2, 1) * dtrb_fltr/2, P, [], [], 1, 3);  % Add forward-path disturbance
% P.OutputName = ["Reference timing", "Reference timing", "Disturbed timing", "Path delay"]; P.OutputUnit = 'femtosecond';
% 
% P = parallel(dtrb_fltr/2, link_dly * G/2, [], [], 1, 1);  % Add half disturbance to forward-trip delayed half-output.
% P.OutputName = "Disturbed path delay"; P.OutputUnit = 'femtosecond';
% P = parallel(meas_noise_fltr, [1; 1] * P, [], [], 1, 2);  % Duplicate channel and add measurement noise to second one.
% P.OutputName = ["Measured disturbed time", "Disturbed time"]; P.OutputUnit = {'femtosecond', 'femtosecond'};
% P = parallel([1; 1] * ref_fltr, -P, [], [], [1, 2], [1, 2]);  % Negate measurement and output and add reference input to yield measured error and real error.
% P = [0 1; 1 0] * P;  % swap output order;
% P.OutputName = ["Time error", "Measured time error"]; P.OutputUnit = {'femtosecond', 'femtosecond'};
% 
% P.Notes = ["Generalized model of laser-lock PLL.";
%            G.Notes];