% ISSTABLE_DLY
% Multi-method robust stability test for time-delay systems.
%
% Arguments
% ---------
% P              : Generalized LTI system mapping exogenous inputs to controlled outputs (in that order). The internal delays should be set to the worst case value.
% Method         : Either of "LMI", "IQCLAB", "NS". First two should produce equivalent LMI problems & results.
% DlyMultMask    : NxM or MxN (N <= M) binary mask assigning internal delays to individual delay robustness multipliers, where N is the number of delay multipliers used and M is the number of internal delays in P. Each delay can only be assigned to one multiplier. If left empty, an independent multiplier is used for each delay. Each delay multiplier is designed for the largest assigned delay.
% DlyMultOrder   : 1xN vector specifying the order of the delay multipliers. If a scalar is given, the order is used for all multipliers.
% DlyMultBasisFct: Selects one of 4 basis functions.
% DlyMultPole    : Sets the pole location for the basis function. Not applicable for the NS method.
% DlyMultEps     : Small positive constant to be used in the bounding transfer function for the delay multipliers. Fixed to 1e-6 for the IQCLAB method.
% LmiTol         : For methods LMI and IQCLAB, sets the definitness tolerance.
% SdpOpts        : For the methods LMI and IQClab, sets the yalmip options.
%
%
% Outputs
% -------
% flag: True if P is certified stable, false otherwise.
% info: Debugging information from the SDP/NSOP setup and solution.
%
% 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 [flag, info] = isstable_dly(P, opts)
    arguments
        P lti
        opts.Method          (1,1) string {matlab.system.mustBeMember(opts.Method, ["LMI", "IQCLAB", "NS"])} = "LMI"
        opts.DlyMultMask logical = []
        opts.DlyMultOrder    (1,:) double {mustBeInteger,mustBeNonnegative}                          = 2
        opts.DlyMultBasisFct (1,1) double {mustBeInteger, mustBeInRange(opts.DlyMultBasisFct, 1, 4)} = 1
        opts.DlyMultPole     (1,1) double {mustBeInteger, mustBeNegative}                            = -1
        opts.DlyMultEps      (1,1) double {mustBePositive}                                           = sqrt(eps)
        opts.LmiTol          (1,1) double {mustBeNonnegative}                                        = sqrt(eps)
        opts.SdpOpts     = sdpsettings('solver', 'mosek', 'debug', 0, 'verbose', 0, 'radius', 1e9)
        opts.SystuneOpts = systuneOptions()
    end
    opts.Method = upper(opts.Method);

    if opts.Method == "IQCLAB" && opts.DlyMultEps ~= 1e-6
        warning("DlyMultEps is fixed to 1e-6 for method 'IQCLAB'!");
        opts.DlyMultEps = 1e-6;
    end

    [nz, nw] = size(P);
    [P, tau] = getDelayModel(P);
    ndly = numel(tau);
    nq = ndly; np = ndly;

    widx = 1:nw; zidx = 1:nz;
    pidx = nw+(1:np); qidx = nz+(1:nq);
    P = P([qidx, zidx], [pidx, widx]); % bring IOs into correct order [q; z] = P * [p; w]
    pidx = 1:np; qidx = 1:nq; %#ok<NASGU>
    widx = nq+(1:nw); zidx = np+(1:nz); %#ok<NASGU>

    % Loop shift
    loop_shift = kron([0, 1; 1 1], eye(ndly));
    P = lft(loop_shift, P, ndly, ndly);

    epsilon = opts.DlyMultEps;
    bft     = opts.DlyMultBasisFct;
    p       = opts.DlyMultPole;
    N       = opts.DlyMultOrder;
    M       = opts.DlyMultMask;
    tol     = opts.LmiTol;

    if isempty(M), M = logical(eye(ndly)); end
    assert(any(size(M) == ndly), "DlyMultMask must have one dimension the same size as P has internal delays!");
    if size(M, 2) ~= ndly, M = M'; end
    assert(isequal(sum(M), ones(1, ndly)), "DlyMultMask must assign each internal delay to exactly one multiplier!");

    ndlymult = size(M, 1);
    if isscalar(N), N = N * ones(ndlymult, 1); end
    assert(numel(N) == ndlymult, "DlyMultOrder must be scalar or be compatible in size to DlyMultMask!");

    % rearange delays channels so that delays belonging to the same multiplier are adjacent.
    [M, uncperm] = sortrows(M', "descend"); M = M'; uncperm = uncperm';
    P = P(uncperm, uncperm);
    tau = tau(uncperm);
    dly = max(M * diag(tau), [], 2);
    rep = sum(M, 2);

    switch opts.Method
        case "LMI"
            isstable_dly_lmi()
        case "IQCLAB"
            isstable_dly_iqclab()
        case "NS"
            isstable_dly_ns()
        otherwise
            error("Unsupoorted method '%s'!", opts.Method);  % Should never happen because of function argument validation.
    end

    function isstable_dly_lmi()
        % Pi = Psi' * Sigma * Psi
        Psi = [];
        Pi = [];

        % Constraints
        cnst = [];

        info = struct(sdp_vars=struct());
        for i = 1:ndlymult
            l = N(i)+1; r = rep(i); d = dly(i);

            basis = fBasis(l, p, r, bft); basis.TimeUnit = P.TimeUnit;
            W = fKron(fW(d, epsilon, 3), eye(r)); W.TimeUnit = P.TimeUnit;
            phi1 = basis * W;

            D = sdpvar(l*r); info.sdp_vars.("D"+i) = D;
            R = sdpvar(l*r); info.sdp_vars.("R"+i) = R;
            X = sdpvar(N(i)*r); info.sdp_vars.("X"+i) = X;

            Pi = blkdiag(Pi, blkdiag(D, ftriang(-R, -R, -R-D, 2)));
            Psi1 = [phi1; basis];
            Psi2 = basis;
            Psi = blkdiag(Psi, blkdiag(Psi1, Psi2));

            % Pi11 = blkdiag(D, zeros(l*r));
            % Pi12 = blkdiag(zeros(l*r), R);
            % Pi22 = blkdiag(R, -D);
            % Pi = blkdiag(Pi, [Pi11, Pi12; Pi12, Pi22]);
            % Psi1 = [phi1; basis];
            % Psi2 = [eye(l*r); eye(l*r)] * basis;
            % Psi = blkdiag(Psi, blkdiag(Psi1, Psi2));

            U = fss2m(basis);
            V = blkdiag(fOblkdiag(X), D);
            cnst = cnst + [U'*V*U >= tol * eye(size(U, 2))]; %#ok<NBRAK1>

            if any(P.D(M(i,:), M(i,:)), "all")
                Y = sdpvar(N(i)*r); info.sdp_vars.("Y"+i) = Y;
                %V = blkdiag(fOblkdiag(Y), -D, -R);
                V = blkdiag(fOblkdiag(Y), -R-D);
                U = fss2m(Psi2);
                cnst = cnst + [U'*V*U <= -tol * eye(size(U, 2))]; %#ok<NBRAK1>
            end
        end; clear i l r d;

        % Build permuted plant index
        idx = [];
        cntr = 0;
        for i = 1:ndlymult
            idx = [idx, cntr+(1:rep(i)), cntr+(1:rep(i))+ndly]; %#ok<AGROW>
            cntr = cntr + rep(i);
        end; clear i cntr;

        % Build extended plant
        G = [P; eye(ndly)];
        G = G(idx, :);
        G = Psi * G;

        X = sdpvar(order(G)); info.sdp_vars.X = X;
        U = fss2m(G);
        V = blkdiag(fOblkdiag(X), Pi);
        cnst = cnst + [U'*V*U <= -tol * eye(size(U, 2))]; %#ok<NBRAK1>

        info.sol_diag = optimize(cnst, [], opts.SdpOpts);
        flag = (info.sol_diag.problem == 0);
        if opts.SdpOpts.verbose
            check(cnst);
        end
        info.constraints = cnst;
        info.sdp_vars = structfun(@(s) value(s), info.sdp_vars, UniformOutput=false);
    end

    function isstable_dly_iqclab()
        Delta = {};
        for i = 1:ndlymult
            delta = iqcdelta(char("dly" + i), InputChannel=find(M(i,:)), OutputChannel=find(M(i,:)), StaticDynamic='D', DelayType=1, DelayTime=dly(i));
            Delta{end+1} = iqcassign(delta, 'udel', Length=N(i)+1, PoleLocation=opts.DlyMultPole, BasisFunctionType=opts.DlyMultBasisFct, AddIQC='yes'); %#ok<AGROW>
        end

        prob = iqcanalysis(P, Delta, Parser="Yalmip", Solver='mosek', Display='on');
        flag = (prob.gamma == "feasible");
        info = struct(prob=prob);
    end

    function isstable_dly_ns()
        Sigma = tf();
        phi0 = tf();
        Psi = tf();

        for i = 1:ndlymult
            r = rep(i); d = dly(i);
            Sigma_ = ss([], TimeUnit=P.TimeUnit);
            phi0_ = tf(zeros(r), TimeUnit=P.TimeUnit);
            Psi_ = tf(zeros(r), TimeUnit=P.TimeUnit);

            % 1 - Circle criterion
            rep = build_tunable_param_matrix(r, "R" + i + "_", "ltrig");
            Sigma_ = [Sigma_; zeros(r)];
            phi0_ = phi0_ - R'*R;
            Psi_ = Psi_ - (R'*R)/2;

            % 2 - Variable radius circle criterion
            %W = 2*tf([d*d, 3.54*d, 1e-6], [d*d, 4.5*d, 7.05], TimeUnit=P.TimeUnit);
            W = fW(d, epsilon, 3); W.TimeUnit = P.TimeUnit;
            D = build_tunable_param_matrix(r, "D" + i + "_", "ltrig");
            Sigma_ = [Sigma_; D*W];
            Psi_ = Psi_ - (D'*D)/2;

            % % 3
            % X = build_tunable_param_matrix(r, "Yd" + i + "_3_");
            % W = tf([-2.19*d*d, 9.02*d, 0.089], [d*d, -5.64*d, -17], TimeUnit=P.TimeUnit);
            % Sigma_ = [Sigma_; zeros(r)];
            % phi0_ = phi0_ + (X'*X)*W;
            % Psi_ = Psi_ - (X'*X)/2;

            % add
            Sigma = append(Sigma, Sigma_);
            phi0 = append(phi0, phi0_);
            Psi = append(Psi, Psi_);
        end; clear Sigma_ phi0_ Psi_;

        sigma_max_goal = TuningGoal.Gain('v', 'c', 1);
        G = P;

        ns = size(Sigma, 1);
        sys = sect(-[phi0 * G + Psi, zeros(ndly, ns); ...
                     Sigma * G, -eye(ns) / 2]);
        sys.InputName = 'v'; sys.OutputName = 'c';

        [sys, fsoft, ~, info] = systune(sys, sigma_max_goal, [], opts.SystuneOpts);
        figure(); sigma(sys);
        flag = (fsoft <= 1 - opts.LmiTol);
    end
end
