% BUILD_LINK_MODEL
% Constructs a parameterized dynamic model for a stabilized timing link setup from system parameters.
%
% Model time unit is seconds unless changed by `time_unit`, model output will be scaled in
% 10^sys_params.scaling unless changed by `scaling`. It is assumed that all parameters in sys_params are consistent
% (e.g. sys_params.k_det should be scaled according to sys_params.scaling). Returns new system structure with
% rescaled models. Output disturbance cannot be attributed to a specific point on the link, hence we assume that the
% endstation sees exactly half, which is true if the disturbance is small for high frequencies (i.e. not really affected
% by the delay on the link).
%
% Arguments
% ---------
% sys_params    : Standardized structure of laser-lock setup parameters.
% Scaling       : How the units of the resulting model should be scaled.
% TimeUnit      : Time unit in which the LTI models are scaled. Default: "seconds"
% FilterMinDecay: Ensures stability of colouring filters by assigning real part to -FilterMinDecay.
% Bandlimit     : Ensures spectral radius of system matrix by moving all poles beyond Bandlimit back to Bandlimit.
%
% Outputs
% -------
% sys: New standardized structure of timing link parameters, including models.
%
% 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 sys = build_link_model(sys_params, args)
    arguments
        sys_params
        args.Scaling        (1,1) double {mustBeFinite, mustBeInteger} = sys_params.scaling
        args.TimeUnit       (1,1) string                               = "seconds"
        args.FilterMinDecay (1,1) double {mustBeNonnegative}           = 0
        args.Bandlimit      (1,1) double {mustBePositive}              = inf
    end

    if mod(args.Scaling, 3)
        error("Scaling must be divisible by 3!");
    end

    time_unit = args.TimeUnit;
    time_unit = ltipack.matchKey(time_unit, ltipack.getValidTimeUnits());
    if isempty(time_unit), error(message('Control:ltiobject:chgunit1')); end

    sys = sys_params;  % Make copy of system

    % Set default parameters, if not provided
    if ~isfield(sys, "pade_order"), sys.pade_order = 1; end
    if ~isfield(sys, "vel_fctr"), sys.vel_fctr = 1/1.47; end
    if ~isfield(sys, "input_scaling"), sys.input_scaling = 1; end

    % Set new scaling
    sys.scaling = args.Scaling;
    sys.scaling_str = num2sip(10^-sys.scaling, [], true);
    sys.scaling_str = extractAfter(sys.scaling_str, find(sys.scaling_str == ' ', 1, 'last')) + "seconds";

    sys.k_det = sys.k_det * 10^(sys.scaling - sys_params.scaling);
    sys.k_act = sys.k_act * 10^(sys.scaling - sys_params.scaling);


    sys.Fd = sys.Fd * 10^(sys.scaling - sys_params.scaling);
    sys.Fd.InputName = "w(1)"; sys.Fd.OutputName = "d";
    sys.Fd.OutputUnit = sys.scaling_str;
    sys.Fd = chgTimeUnit(sys.Fd, time_unit);
    sys.Fd.UserData = sys_params.Fd.UserData;

    sys.Fn = sys.Fn * 10^(sys.scaling - sys_params.scaling);
    sys.Fn.InputName = "w(2)"; sys.Fn.OutputName = "n";
    sys.Fn.OutputUnit = sys.scaling_str;
    sys.Fn = chgTimeUnit(sys.Fn, time_unit);
    sys.Fn.UserData = sys_params.Fn.UserData;

    sys.C = sys.C * 10^-(sys.scaling - sys_params.scaling);
    sys.C.InputName = "e"; sys.C.OutputName = "u";
    sys.C.InputUnit = sys.scaling_str; sys.C.OutputUnit = "V";
    sys.C = chgTimeUnit(sys.C, time_unit);
    sys.C.UserData = sys_params.C.UserData;

    sys.G_pz = chgTimeUnit(zpk(sys.G_pz), time_unit);

    tconv = tunitconv("seconds", time_unit);
    sys.Fd = ensure_limits(sys.Fd, args.Bandlimit / tconv, 1 / tconv, "cut");
    sys.Fn = ensure_limits(sys.Fn, args.Bandlimit / tconv, 1 / tconv, "cut");
    %TODO: sys.G_pz = ensure_limits(sys.G_pz, args.Bandlimit / tconv, 0, "cut");

    if sys.drv_eff_bw > args.Bandlimit, sys.drv_eff_bw = inf; end
    if sys.drv_lp_fc > args.Bandlimit, sys.drv_lp_fc = inf; end

    % Build piezo driver model
    p = [sys.drv_eff_bw, sys.drv_lp_fc];
    sys.G_drv = zpk([], -2*pi * p(isfinite(p)), 1);
    sys.G_drv.K = sys.drv_gain / dcgain(sys.G_drv);  % Set DC-Gain to sys.drv_gain
    if 2/sys.loop_dly <= args.Bandlimit
        sys.G_drv.InputDelay = sys.loop_dly;
    end
    sys.G_drv = chgTimeUnit(sys.G_drv, time_unit);
    sys.G_drv.InputName = "u"; sys.G_drv.InputUnit = "V";
    sys.G_drv.OutputName = "U"; sys.G_drv.OutputUnit = "V";

    % Take piezo impedance model from measured response
    G_pz = sys.G_pz;
    if ~all(sys.G_pz.P{1})
        error("G_pz must model resonances only, not the capacitive 1/f behaviour.")
        % G_pz.P{1} = G_pz.P{1}(sys.G_pz.P{1} ~= 0);  % Remove integral behaviour of capacitor-like piezo impedance, cancels out anyway and inhibts usage of dcgain.
    end
    if dcgain(sys.G_pz) - 1 > sqrt(eps)
        warning("G_pz has non-unity DC gain, will be ignored.")
        G_pz = sys.G_pz / dcgain(sys.G_pz);  % DC gain of G_pz is absorbed in k_act. We just need this for G_l, so don't store in sys.
    end

    % Fiber stretcher dynamics
    sys.G = sys.k_act * G_pz;
    sys.G = chgTimeUnit(sys.G, time_unit);
    sys.G.InputName = "U"; sys.G.InputUnit = "V";
    sys.G.OutputName = "phi"; sys.G.OutputUnit = "fs";

    % Include driver
    sys.G = sys.G * sys.G_drv;
    sys.G.K = sys.G.K * sys.input_scaling;

    % Include delay
    speed_of_light = 3e8;  % m/s
    sys.link_dly = sys.link_len / speed_of_light / sys.vel_fctr;
    if 1/sys.link_dly >= args.Bandlimit, sys.link_dly = 0; end
    % assuming next that correlation time of d is much larger than delay.
    sys.P = connect(sys.G, sumblk("fwd = r + phi"), sumblk("y = fwd_dly + d"), sumblk("bwd = y + d"), sumblk("e = r - bwd_dly - phi + n"), ...
                    chgTimeUnit(ss(1, OutputDelay=sys.link_dly, InputName="fwd", OutputName="fwd_dly"), time_unit), ...
                    chgTimeUnit(ss(1, OutputDelay=sys.link_dly, InputName="bwd", OutputName="bwd_dly"), time_unit), ...
                    {'r', 'd', 'n', 'u'}, {'y', 'e'});
    sys.P = sys.P * ss(diag([1, 1/2, 1, 1/2]), InputName=sys.P.InputName, TimeUnit=time_unit); % d and u act on fwd and bwd path, each path sees half the effect.
    sys.Pcol = connect(sys.P, sys.Fd, sys.Fn, {'w', 'r', 'u'}, {'y', 'e'});

    sys.CL = lft(sys.P, sys.C / sys.input_scaling * AnalysisPoint('e'));
    sys.CLcol = lft(sys.Pcol, sys.C / sys.input_scaling * AnalysisPoint('e'));
end
