% H2SYN_NS_TDS
% Synthesize structured H2-suboptimal output-feedback controller using nonsmooth optimization with constant time-delay robustness.
%
% Arguments
% ---------
% P            : Generalized LTI system mapping exogeneous inputs and control inputs to controlled outputs and measured outputs (in that order). The internal delays should be set to the worst case value.
% K            : Tunable LTI system used to close the control channel of P as CL = lft(P, K). Implicitly defines number of control and measured output channels in P.
% PerfMultOrder: Order of the H2 performance multiplier.
% PerfMultReal : If true, constraints the poles of the performance multiplier to be real.
% PerfMultDiag : If true, constraints the performance multiplier matrix to be diagonal.
% 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.
% DlyMultEps   : Small positive constant to be used in the bounding transfer function for the delay multipliers.
% DlyMultInit  : Block structure with initial values for the delay multipliers.
% SystuneOpts  : Options to be used for the systune call.
%
% Outputs
% -------
% CL   : Closed-loop system obtained by closing the control channel y -> u of P with K in LFT fashion.
% K    : Synthesized output-feedback controller.
% gamma: Achieved H2-performance bound.
% info : Debugging information from the SDP setup and solver 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 [K, CL, gamma, info] = h2syn_ns_tds(P, K, opts)
    arguments
        P ss
        K genlti
        opts.PerfMultOrder (1,1) double {mustBeInteger,mustBePositive} = 2
        opts.PerfMultReal  (1,1) logical                               = false
        opts.PerfMultDiag  (1,1) logical                               = false
        opts.DlyMultMask   (:,:) logical                               = []
        opts.DlyMultEps    (1,1) double {mustBePositive}               = 1e-6
        opts.DlyMultInit   (1,:) struct = struct()
        opts.SystuneOpts = systuneOptions(MaxIter=1e3, RandomStart=25, Display="final")
    end

    [nu, ny] = size(K);
    nw = size(P, 2) - nu;
    nz = size(P, 1) - ny;

    P_orig = P;
    [P, tau] = getDelayModel(P);
    ndly = numel(tau);
    nq = ndly;
    np = ndly;

    widx = 1:nw; uidx = nw+(1:nu); pidx = nw+nu+(1:np);
    zidx = 1:nz; yidx = nz+(1:ny); qidx = nz+ny+(1:nq);
    P = P([qidx, zidx, yidx], [pidx, widx, uidx]); % bring IOs into correct order [q; z; y] = P * [p; w; u]
    qidx = 1:nq; widx = nq+(1:nw); uidx = nq+nw+(1:nu);
    pidx = 1:np; zidx = np+(1:nz); yidx = np+nz+(1:ny);

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

    %% Augmented Multiplier
    Sigma = tf();
    Psi = tf();
    phi0 = tf();

    % Constant Delay
    epsilon = opts.DlyMultEps;

    M = opts.DlyMultMask;
    if isempty(M), M = 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);

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

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

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

        % 2 - Variable 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 = CL.TimeUnit;
        D = build_tunable_param_matrix(r, "D" + i + "_", "ltrig");
        D = replaceBlock(D, opts.DlyMultInit);
        Sigma_ = [Sigma_; D*W];
        Psi_ = Psi_ - (D'*D)/2;

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

    %- Performance
    %-- H2
    PsiY = 0;
    for i = 1:opts.PerfMultOrder
        X = realp("X" + i, zeros(nw));
        if opts.PerfMultDiag, X.Free = eye(nw); end
        a = realp("a" + i, 1);
        a.Minimum = 0;
        if opts.PerfMultReal
            tmp = X * tf(1, [1, a]);
        else
            Y = realp("Y" + i, zeros(nw));
            b = realp("b" + i, 0);
            b.Minimum = 0;
            tmp = X * tf([1, a], [1, 2*a, a*a + b*b]) + Y * tf(b, [1, 2*a, a*a + b*b]);
        end
        PsiY = PsiY + tmp;
    end; clear X Y a b i;
    PsiY.InputName = 'k'; PsiY.OutputName = 'l';
    PsiY = chgTimeUnit(PsiY, CL.TimeUnit);

    Sigma = append(Sigma, eye(nz));
    phi0 = append(phi0, zeros(nw, nz));
    Psi = append(Psi, -PsiY);


    %% Goals
    sigma_max_goal = TuningGoal.Gain('v', 'c', 1);
    h2_goal = HermIntegral('k', 'l', 1);

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

    [sys, fsoft, ghard, runinfo] = systune(append(sys, PsiY), h2_goal, sigma_max_goal, opts.SystuneOpts);
    if ghard >= 1 + sqrt(eps)
        warning("Failed to enforce constraints!");
    end

    runinfo_hotstart = runinfo(1);
    runinfo_rand = runinfo(2:end);
    [~, opt_idx] = sort([runinfo_rand.gHard]);
    runinfo_rand = runinfo_rand(opt_idx);

    info = struct(hotstart_run=runinfo_hotstart, rand_runs=runinfo_rand, PsiY=replaceBlock(PsiY, sys.Blocks), sigma_max_sys=sys, P=P_orig);

    K = replaceBlock(K, sys.Blocks);
    CL = ss(lft(P_orig, K));
    gamma = sqrt(fsoft);
    if ~isreal(gamma), gamma = inf; end
end
