% H2SYN_SI_LPV Synthesize structured H2-suboptimal output-feedback controller robust against LPV uncertainty using
% full-block multipliers, the full-block S-procedure and the sparsity invariance frame.
%
% 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, gamma, Jnom, sol_diag] = h2syn_si_lpv(M, Delta, BlkStruct, S, epsilon, options)
    arguments
       M ss
       Delta uss
       BlkStruct struct
       S double {mustBeMember(S, [0 1])}
       epsilon (1,1) double {mustBeNonnegative} = 1e-9
       options.solver (1,1) string {mustBeNonzeroLengthText(options.solver)} = "mosek"
       options.verbose (1,1) double {mustBeNonnegative,mustBeInteger} = 1
       options.debug (1,1) double {mustBeNonnegative,mustBeInteger} = 1
    end

    % yalmip('clear'); % Don't clear user's variables

    % Extract matrices.
    nx = order(M);
    pidx = startsWith(M.InputName, 'p'); np = sum(pidx);
    widx = startsWith(M.InputName, 'w'); nw = sum(widx);
    uidx = startsWith(M.InputName, 'u'); nu = sum(uidx);
    qidx = startsWith(M.OutputName, 'q'); nq = sum(qidx);
    zidx = startsWith(M.OutputName, 'z'); nz = sum(zidx);
    yidx = startsWith(M.OutputName, 'e'); ny = sum(yidx);

    Cy = M.C(yidx, :);
%     if any(Cy & ~[zeros(nu, nx-ny), eye(ny)])
%         warning("Plant is not in output-state form! Will transform automatically, but manual numerical preconditioning is prefered.");
%         M = balreal(M);
%         M = output_state_transform2(M, 'e');
%         M = truncate_coeff(M, 1e-20);
%         M = prescale(M);
%     end

    A = M.A;
    Bp = M.B(:, pidx); Bw = M.B(:, widx); Bu = M.B(:, uidx);
    Cq = M.C(qidx, :); Cz = M.C(zidx, :); Cy = M.C(yidx, :);
    Dqp = M.D(qidx, pidx); Dqw = M.D(qidx, widx); Dqu = M.D(qidx, uidx);
    Dzp = M.D(zidx, pidx); Dzw = M.D(zidx, widx); Dzu = M.D(zidx, uidx);
    Dyp = M.D(yidx, pidx); Dyw = M.D(yidx, widx); Dyu = M.D(yidx, uidx);

    % Verify
    assert(size(Delta, 1) == np && size(Delta, 2) == nq, "Size of delta incompatible with p -> q channel of M");
    assert(~any(Dzw(:)), "H2-performance not finite with feedthrough from w -> z");
    assert(~any(Dyp(:)), "Feedthrough in p -> e not allowed, find different LFR");
    assert(~any(Dyw(:)), "Feedthrough in w -> e not allowed, add artifical pole");
    assert(~any(Dyu(:)), "Feedthrough in u -> e not allowed, add artifical pole");
    assert(isequal(size(S), [nu, ny]));

    % Optimization Variables
    %- Sparsity Invariance Structure Matrices
    S = [zeros(nu, nx-ny), S];
    T = S;
    R = si_opt_r(T);
    assert(isequal(R, R^(nx-1) > 0), "R can be improved");

    %- Y is not symmetric, so we can use this more elegant, loop-less code.
    [Ti, Tj] = find(T);
    M = sparse(Ti, Tj, sdpvar(sum(T(:)), 1), nu, nx);
    clear Ti Tj;

    %- X needs to be symmetric, no other way known to do that without loop.
    Y = sdpvar(nx);
    for j = 1:size(R, 1)
        for k = 1:size(R, 2)
            if ~R(j,k)
                Y(j,k) = 0;
            end
        end
    end; clear j k;

    %- H2-performance slack variable
    Z = sdpvar(nz);

    % Dualized "Stability" Multiplier
    W = sdpvar(np); % TODO: Scherer, 1999 check if off-diagonal blocks of W are zero
    U = sdpvar(np, nq, 'full');
    V = sdpvar(nq);

    % Dualized "Performance" Multiplier
    Wp = sdpvar(np); % TODO: Scherer, 1999 check if off-diagonal blocks of W are zero
    Up = sdpvar(np, nq, 'full');
    Vp = sdpvar(nq);

    % Solver Options
    opt = sdpsettings('solver', options.solver, 'verbose', options.verbose, 'debug', options.debug);

    % Objective
    obj = trace(Z);

    % Constraints
    U_ = [-(A*Y + Bu*M)', -(Cq*Y + Dqu*M)';
          eye(nx), zeros(nx, nq);
          -Bp', -Dqp';
          zeros(nq, nx), eye(nq);
          -Bw', -Dqw'];
    V_ = blkdiag(fOblkdiag(eye(nx)), ...
                 [W, U; ...
                  U', V], ...
                 -eye(nw));

    cnst = [Y >= epsilon * eye(size(Y));
            Z >= epsilon * eye(size(Z));
            U_' * V_ * U_ >= epsilon * eye(size(U_,2));
            [Y, -(Cq*Y + Dqu*M)', -(Cz*Y + Dzu*M)';
             -(Cq*Y + Dqu*M), Dqp*Wp*Dqp' - Dqp*Up - (Dqp*Up)' + Vp, Dqp*Wp*Dzp' - (Dzp*Up)';
             -(Cz*Y + Dzu*M), Dzp*Wp*Dqp' - Dzp*Up, Dzp*Wp*Dzp' + Z] >= epsilon * eye(nx+nq+nz);
            V >= epsilon * eye(size(V));
            Vp >= epsilon * eye(size(Vp))
           ];

    %- Finite dimensional robustness verification
    idx = [0, cumsum([BlkStruct.Occurrences])];
    for i = 1:length(BlkStruct)
        cnst = [cnst;
                V(idx(i)+1:idx(i+1),idx(i)+1:idx(i+1)) >= epsilon * eye(BlkStruct(i).Occurrences);
                Vp(idx(i)+1:idx(i+1),idx(i)+1:idx(i+1)) >= epsilon * eye(BlkStruct(i).Occurrences)]; %#ok<AGROW>
    end; clear i idx;

    % Generate convex hull of uncertainty set
    unc_hull_vrtx = arrayfun(@(x) ones(x, 2) * diag([-1, 1]), [BlkStruct.Occurrences], 'UniformOutput', false);
    unc_hull_vrtx = combvec(unc_hull_vrtx{:});
    M_ = [W, U;
          U', V];
    Mp_ = [Wp, Up;
           Up', Vp];
    for i = 1:size(unc_hull_vrtx,2)
        U_ = [eye(nq); -diag(unc_hull_vrtx(:,i))'];

        cnst = [cnst;
                U_' * M_ * U_ <= -epsilon * eye(nq)]; %#ok<AGROW>
        cnst = [cnst;
                U_' * Mp_ * U_ <= -epsilon * eye(nq)]; %#ok<AGROW>
    end; clear i unc_hull_vrtx;

    % Solve!
    sol_diag = optimize(cnst, obj, opt);
    if options.debug, check(cnst); end
    if options.verbose, disp(sol_diag.info); end

    [primal_residual, dual_residual] = check(cnst);
    if sol_diag.problem == 0 && (any(primal_residual < -epsilon) || any(dual_residual < -epsilon))
        warning("Constraint violated beyond definitness margin!");
        sol_diag.problem = 8;
    end

    try
        K = value(M) / value(Y); % Recover controller.
        K = K(end-ny+1:end, end-ny+1:end);
        K = K ./ diag(Cy(:, end-ny+1:end))';
    catch
        warning("Unable to recover controller, check solver status.");
        K = [];
    end

    gamma = sqrt(value(obj));
    if(~isreal(gamma)), gamma = Inf; end

    %- Nominal Performance
    try
        P = lyap((A+Bu*K*Cy), Bw * Bw');
        Jnom = sqrt(trace((Cz+Dzu*K*Cy) * P * (Cz+Dzu*K*Cy)'));

        % Alternate
        % P = lyap((A+Bu*K*Cy)', (Cz+Dzu*K*Cy)' * (Cz+Dzu*K*Cy));
        % J = sqrt(trace(Bw' * P * Bw));

        if(~isreal(Jnom)), Jnom = Inf; end
    catch
        Jnom = Inf;
    end
end
