% H@LMI_LPV Optimizes a non-conservative H2-norm upper bound using LMI
% methods for systems affected by LPV uncertainty.
% Implements analysis methods from Scherer.
%
% 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 [gamma, info] = h2lmi_lpv(M, Delta, BlkStruct, opts)
    arguments
        M ss % The plant to be analyzed.
        
        Delta uss
        BlkStruct struct 
        
        % Force definitness constraints to satisfy this margin.
        opts.tolerance (1, 1) double {mustBeNonnegative} = 1e-9
        
        % Hard upper bound on H2-norm optimization variable.
        opts.hard_bound (1, 1) double {mustBePositive} = inf
        
        % YALMIP options.
        opts.sdp_options (1, 1) struct = sdpsettings()
    end
    
    
    % Extract matrices.
    nx = order(M);
    pidx = startsWith(M.InputName, 'p'); np = sum(pidx);
    widx = startsWith(M.InputName, 'w'); nw = sum(widx);
    qidx = startsWith(M.OutputName, 'q'); nq = sum(qidx);
    zidx = startsWith(M.OutputName, 'z'); nz = sum(zidx);
    
    tol = opts.tolerance;
    
    A = M.A;
    Bp = M.B(:, pidx); Bw = M.B(:, widx);
    Cq = M.C(qidx, :); Cz = M.C(zidx, :);
    Dqp = M.D(qidx, pidx); Dqw = M.D(qidx, widx);
    Dzp = M.D(zidx, pidx); Dzw = M.D(zidx, widx);
    
    if any(Dzw)
        warning("System has non-zero feedthrough and hence unbounded H2-norm.");
        gamma = inf;
        return;
    end
    
    X = sdpvar(nx); 
    Z = sdpvar(nz);

    Pstab = sdpvar(np+nq);
    Pperf = sdpvar(np+nq);

    U_ = [eye(nx), zeros(nx,np), zeros(nx,nw);
          A, Bp, Bw;
          zeros(np,nx), eye(np), zeros(np,nw);
          Cq, Dqp, Dqw;
          zeros(nw,nx), zeros(nw,np), eye(nw)];
    V_ = blkdiag(fOblkdiag(X), Pstab, -eye(nw));
    cnst = [U_' * V_ * U_ <= -tol * eye(size(U_, 1))];

    U_ = [eye(nx+np);
          Cq, Dqp;
          Cz, Dzp];
    V_ = blkdiag(-X, Pperf, Z);
    cnst = [cnst;
            U_' * V_ * U_ <= -tol * eye(size(U_, 1))];
    
    if isfinite(opts.hard_bound)
        cnst = [cnst;
                gamma_sq <= opts.hard_bound];
    end

    % 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{:});
    for i = 1:size(unc_hull_vrtx,2)
        U_ = [diag(unc_hull_vrtx(:,i)); eye(nq)];

        cnst = [cnst;
                U_' * Pstab * U_ >= tol * eye(nq)]; %#ok<AGROW> % possibly non-strict
        cnst = [cnst;
                U_' * Pperf * U_ >= tol * eye(nq)]; %#ok<AGROW> % possibly non-strict
    end; clear i unc_hull_vrtx;

    sol_diag = optimize(cnst, -trace(Z), opts.sdp_options);

    info = struct();
    info.sol_diag =sol_diag;
    info.constraints = cnst;
    info.X = value(X);
    info.gamma = sqrt(1/value(trace(Z)));
end