% BUILD_PLL_MODEL Constructs a generalized PLL plant [z, e] = P * [w, u] from input parameters.
% Uses default parameters if not overwritten by function arguments.
%
% 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 [P, info] = build_pll_model(G, Fr, Fd, Fn, args)
    arguments
        G (1,1) lti = zpk([], 0, db2mag(80))
        Fr (1,1) lti = tf([1, 10, 100e-3], [1, 0, 0], TimeUnit='milliseconds')  % build_powerlaw_tf(-60, [1e4, 1e1]);
        Fd (1,1) lti = tf([3.162, 316.2, 31.62], [1, 0, 0], TimeUnit='milliseconds')  % build_powerlaw_tf(-50, [1e5, 1e2]);
        Fn (1,1) lti = tf(db2mag(-70))
        args.Bandlimit (1,1) double = inf
        args.Rolloff (1,1) double = 0
        args.TimeUnit (1,1) string = ''
        args.Tolerance = 1e-9
    end
    
    tol = args.Tolerance;
    
    if tol > 0
        G = minreal(G, tol, false);
        Fr = minreal(Fr, tol, false);
        Fd = minreal(Fd, tol, false);
        Fn = minreal(Fn, tol, false);
    end
    
    time_unit = args.TimeUnit;
    if ~strlength(time_unit), time_unit = G.TimeUnit; end
    time_unit = ltipack.matchKey(time_unit,ltipack.getValidTimeUnits());
    if isempty(time_unit), error(message('Control:ltiobject:chgunit1')); end
    tunit_fctr = tunitconv('seconds', time_unit);
    
    bandlimit = args.Bandlimit / tunit_fctr;
    rolloff = args.Rolloff  / tunit_fctr;
    
    % Analysis points
%     ap_u = AnalysisPoint('u');
%     ap_u.Name = 'ap_input';
%     ap_u.Location{1} = 'Control action';
%     ap_u = chgTimeUnit(ap_u, time_unit);
%     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 error';
%     ap_e = chgTimeUnit(ap_e, time_unit);
%     ap_e.InputName = 'Measured error';
%     ap_e.OutputName = ap_e.InputName;
    ap_r = AnalysisPoint('r');
    ap_r.Name = 'ap_reference';
    ap_r.Location{1} = 'r';
    ap_r = chgTimeUnit(ap_r, time_unit);
    ap_r.InputName = 'r';
    ap_r.OutputName = ap_r.InputName;
    
    %% Models
    %- Helpers
    info.rename_ex_inputs = ss(eye(3), InputName='w', OutputName={'wr', 'wd', 'wn'});
    
    %- Voltage-controlled oscillator phase model
    G = chgTimeUnit(G, time_unit);
    G.InputName = 'u'; G.OutputName = 'yy';
    
    %- Noise colouring filters
    %-- Reference noise
    Fr = ensure_limits(Fr);
    Fr.InputName = 'wr'; Fr.OutputName = 'r';
    
    %-- Local noise
    Fd = ensure_limits(Fd);
    Fd.InputName = 'wd'; Fd.OutputName = 'd';
    
    %-- Measurement noise
    bandlimit = inf;  % Trick: We don't need to enforce bandlimit on Fn and don't need the variable hereon.
    Fn = ensure_limits(Fn);
    Fn.InputName = 'wn'; Fn.OutputName = 'n';
    
    %% Generalized plant
    %- Interconnection [y, e] = G * [wr, wd, wn, u];
    warning('off', 'Control:combination:OutputNameClash');
    P = [Fr, -Fd, 0, -G;
         0, Fd, 0, G;
         ap_r*Fr, -Fd, -Fn, -G];  % constructing interconnection with tf/zpk models appears to work better than with ss wrt later numerical properties.
    warning('on', 'Control:combination:OutputNameClash');
    P.InputName = {'w(1)', 'w(2)', 'w(3)', 'u'}; P.OutputName = {'z', 'y', 'e'};
    
    %- Improve
    %P = minreal(P, tol, false);  % Remove redundant states. NOTE: take care of the tolerance parameter.
    %P = truncate_coeff(P, tol);  % Get rid of numerical noise.
    %P = prescale(P);  % Improve numerical conditioning.
    
    %- Extract helper variables
    info.nx = order(P);
    widx = startsWith(P.InputName, 'w'); info.widx = widx; info.nw = sum(widx);
    uidx = startsWith(P.InputName, 'u'); info.uidx = uidx; info.nu = sum(uidx);
    zidx = startsWith(P.OutputName, 'z'); info.zidx = zidx; info.nz = sum(zidx);
    yidx = startsWith(P.OutputName, 'y'); info.yidx = yidx; info.ny = sum(yidx);
    eidx = startsWith(P.OutputName, 'e'); info.eidx = eidx; info.ne = sum(eidx);
    
    info.A = P.A;
    info.Bw = P.B(:,widx);
    info.Bu = P.B(:,uidx);
    info.Cz = P.C(zidx,:);
    info.Cy = P.C(yidx,:);
    info.Ce = P.C(eidx,:);
    info.Dzu = P.D(zidx, uidx);
    info.Dew = P.D(eidx, widx);
    
    info.G = G;
    info.Fr = Fr;
    info.Fd = Fd;
    info.Fn = Fn;
    
    function sys = ensure_limits(sys)
        sys = zpk(chgTimeUnit(sys, time_unit));
        
        sys.P{1}(real(sys.P{1}) == 0) = -2*pi*rolloff; % Move uncontrollable modes on imaginary axis back to rolloff frequency.
        
        if isfinite(bandlimit)
            idx = (abs(sys.P{1}) >= 2*pi*bandlimit);
            sys.P{1}(idx) = sign(sys.P{1}(idx)) * 2*pi*bandlimit;  % Ensure bandlimit by moving poles beyond bandlimit to bandlimit frequency.
            rel_deg = length(sys.Z{1}) - length(sys.P{1});
            sys.P{1} = [sys.P{1}; -2*pi*bandlimit * ones(max(0, rel_deg+1), 1)];  % Ensure no feedthrough w -> z by adding poles at bandlimit frequency till strictly proper.
            sys.K = sys.K * 2*pi*bandlimit^max(0, rel_deg+1);  % Compensate gain
        end
        if tol > 0
            sys = minreal(sys, tol, false);  % Simplify
        end
    end
end