% LASER_PLL Constructs a parametric closed-loop model of a tunable mode-locked laser in a phase-locked loop.
% - 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.
% - Notes is used as description in bullet point list format.
%
% 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>

function CL = laser_pll(name)
    arguments
        name (1,1) string = ""
    end

    prefix = lower(matlab.lang.makeValidName(name)) + "_";
    
    % Convenience stuff
    ms = 'milliseconds';
    fs = 'femtoseconds';
    tu = ms;  % Target time unit
    s = chgTimeUnit(tf('s'), tu);
    
    % Analysis points
    ap_u = AnalysisPoint(prefix + "u");
    %ap_u.Location{1} = 'Control action';
    ap_u = chgTimeUnit(ap_u, tu);
    ap_u.InputName = name + "Control action";
    ap_u.OutputName = ap_u.InputName;
    ap_e = AnalysisPoint(prefix + "e");
    %ap_e.Location{1} = 'Measured timing error';
    ap_e = chgTimeUnit(ap_e, tu);
    ap_e.InputName = name + "Measured timing error";
    ap_e.OutputName = ap_e.InputName;
    ap_e.InputUnit = 'femtosecond';
    ap_e.OutputUnit = ap_e.InputUnit;
    ap_r = AnalysisPoint(prefix + "r");
    %ap_r.Location{1} = 'Reference timing';
    ap_r = chgTimeUnit(ap_r, tu);
    ap_r.InputName = name + "Reference timing";
    ap_r.OutputName = ap_r.InputName;
    ap_r.InputUnit = 'femtosecond';
    ap_r.OutputUnit = ap_r.InputUnit;
    
    % Laser cavity piezo tuning
    % 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) explcitily.
    % Q = \int U / Z dt
    % We just consider the resonance dynamics and the overall tuning gain.

    pz_res = tunableTF(prefix + "pz_res", 0, 0);  % Base model of perfect piezo response with no resonance.
    pz_res.Numerator.Value = 1; pz_res.Numerator.Free = 0;
    pz_res.Denominator.Value = 1; pz_res.Denominator.Free = 0;
    pz_res = chgTimeUnit(pz_res, tu);  % Change to target timeunit;
    pz_res.Notes = ["Piezo actuator resonance model (model excluding purely capacitive (integral) dynamics).";
                    "No reasonance."];
    
    % Laser dynamics
    f_rep = realp(prefix + "f_rep", 1.3e9/6);  % Nominal repetition rate of the mode-locked laser oscillator (Hz).
    f_rep.Free = 0; f_rep.Minimum = 0;
    k_rep = realp(prefix + "k_rep", 12);  % Induced change in repetition rate due to applied tuning voltage (Fine tuning gain Hz/V).
    k_rep.Free = 0;
    laser = k_rep * tunitconv('seconds', fs) / f_rep / (pz_res / dcgain(pz_res)) / s;  % remove dcgain of pz_chrg as it is included in k_rep by identification.
    laser.InputName = name + "Tuning voltage";
    laser.InputUnit = 'volt';
    laser.OutputName = name + "Timing";
    laser.OutputUnit = 'femtosecond';
    laser.Notes = ["Model of a generic (e.g. One-Five Origami-15) mode-locked laser oscillator.";
                   "`k_rep` is the induced change in repitition rate (in Hz) per volt applied to the laser cavity tuning piezo.";
                   "`f_rep` is the nominal repetition rate of the mode-locked laser oscillator when the nominal bias voltage is applied to the tuning input.";];
    
    
    % Driver (PZT4)
    pz_c = realp(prefix + "pz_c", 300e-9);  % Load (piezo) capacitance, 300 nF typical for Origami-15
    pz_c.Free = 0; pz_c.Minimum = 0;
    drv_out_imp = realp('drv_out_imp', 5);  % Output impedance of the PZT4 piezo amplifier (Ohm).
    drv_out_imp.Free = 0; drv_out_imp.Minimum = 0;
    drv_lp_fc = realp(prefix + "drv_lp_fc", 50e3);  % Corner frequency of the PZT4 configurable low-pass filter (Hz).
    drv_lp_fc.Free = 0; drv_lp_fc.Minimum = 0;
    drv_gain = realp(prefix + "drv_gain", 40);  % Gain of the PZT4 piezo driver in unipolar mode (V/V).
    drv_gain.Free = 0;
    
    drv = drv_gain * tf(1, [1/(2*pi*drv_lp_fc), 1]) * tf(1, [drv_out_imp * pz_c, 1]);
    drv = chgTimeUnit(drv, tu);
    drv.InputName = name + "Input voltage";
    drv.InputUnit = 'volt';
    drv.OutputName = name + "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_lp_fc` is the user configurable corner frequency of the first-order output low-pass filter.";
                 "`driver_out_imp` is the output impedance model of the amplifier."];
    
    
    % Output Disturbance
    dtrb_fltr = zpk(-1.454e5, [-1.688, -2555], 8.94 * 6.116, ...  % "Adapted from MLO phase noise identified in M. Heuer thesis with proper 1/f noise and removed measurement (measurement) noise floor"
                    InputName=name + "White disturbance noise", ...
                    OutputName=name + "Timing disturbance", ...
                    OutputUnit='femtosecond');
    dtrb_fltr = tunableTF(prefix + "dtrb_fltr", tf(dtrb_fltr));
    dtrb_fltr.Numerator.Free = 0;
    dtrb_fltr.Denominator.Free = 0;
    dtrb_fltr = chgTimeUnit(dtrb_fltr, tu);
    dtrb_fltr.Notes = "Adapted from MLO phase noise identified in M. Heuer thesis with removed (measurement) noise floor";
    
    
    % Measurement Noise
    meas_noise_fltr = tf(db2mag(-52), 1, ...
                         InputName=name + "White measurement noise", ...
                         OutputName=name + "Measurement noise", ...
                         OutputUnit='femtosecond');
    meas_noise_fltr = tunableTF(prefix + "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 = ["MZM detection chain noise coloring filter.";
                             "Estimated as flat floor integrating to ~0.1 fs jitter from 10 to 2^15/2 Hz (FW DAQ rate)."];

    
    % Connect open-loop plant
    G = laser * drv;
    G.Notes = [laser.Notes;
               drv.Notes];
    
    
    % Connect generalized plant
    warning('off', 'Control:combination:OutputNameClash');
    P = parallel(dtrb_fltr, G, [], [], 1, 1);  % Add disturbance to output.
    warning('on', 'Control:combination:OutputNameClash');
    P.OutputName = name + "Disturbed timing";
    P.OutputUnit = 'femtosecond';
    P = parallel(meas_noise_fltr, [1; 1] * P, [], [], 1, 1);  % Duplicate channel and add measurement noise to first one.
    P.OutputName = [name + "Measured disturbed timing", name + "Disturbed timing"];
    P.OutputUnit = {'femtosecond', 'femtosecond'};
    P = parallel(ap_r, diag([-1, 1]) * P, [], [], 1, 1);  % Negate measurement and add reference input to yield measured error.
    P = [0 1; 1 0] * P;  % swap output order;
    P.OutputName = [name + "Disturbed timing", name + "Measured timing error"];
    P.OutputUnit = {'femtosecond', 'femtosecond'};
    
    P.Notes = ["Generalized model of laser-lock PLL.";
               G.Notes];
    
    
    % Controller
    ctrl = pid(3e-5, 375e-6, ...
               InputName=name + "Measured timing error", ...
               InputUnit='femtosecond', ...
               OutputName=name + "Control action");
    ctrl = tunablePID('ctrl', ctrl);
    ctrl = chgTimeUnit(ctrl, tu);
    ctrl.UserData = struct(update_rate=1.3e9/16/48);
    ctrl.Notes = ["Scaled, continuous-time approximation of LbSync RAPID-X laserlock firmware PID phase controller.";
                  "Output range is [-1,1] volts";
                  "Controller rate calculated as 1.3e9 / 16 / 48 from documentation in M. Heuer thesis on RAPID-X laser lock firmware."];
    
    % Connect closed loop plant
    CL = lft(P, ap_u * ctrl * ap_e);
    CL.Notes = [P.Notes; ctrl.Notes];
end