% H2SYN_LMI
% Synthesize a H2 (sub)optimal output-feedback controller using LMI methods.
%
% Implements LMI synthesis methods from Sato & Liu.
%
% Arguments
% ---------
% P   : Generalized plant in the form [z; y] = P [w; u].
% ny  : Number of measured outputs.
% nu  : Number of control inputs.
% opts: h2syn_lmi_options controlling the SDP setup.
%
% Outputs
% -------
% K    : Synthesized output-feedback controller.
% CL   : Closed-loop system obtained by closing the control channel y -> u of P with K in LFT fashion.
% 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_lmi2(P, ny, nu, opts)
    arguments
        P ss
        ny   (1,1) double {mustBeInteger,mustBePositive}
        nu   (1,1) double {mustBeInteger,mustBePositive}
        opts (1,1) struct = h2syn_lmi_options()
    end

    mustBeNonnegative(opts.tolerance);
    tol = opts.tolerance;
    mustBeGreaterThanOrEqual(opts.relaxation_factor, 1);
    rlx_fctr = opts.relaxation_factor;
    mustBeGreaterThanOrEqual(opts.regularization_factor, 1);
    reg_fctr = opts.regularization_factor;
    mustBeNonnegative(opts.norm_cost_factor);
    nrm_cst_fctr = opts.norm_cost_factor;
    mustBePositive(opts.hard_bound);
    if ~opts.minimize_gamma && ~isfinite(opts.hard_bound)
        warning("Non-finite hard-bound implies minimization of gamma!")
        opts.minimize_gamma = true;
    end

    info = struct();

    nx = size(P.A, 1);
    nu = nu;
    nw = size(P.B, 2) - nu;
    ny = ny;
    nz = size(P.C, 1) - ny;

    widx = 1:nw;
    uidx = nw+1:nw+nu;
    zidx = 1:nz;
    yidx = nz+1:nz+ny;

    A = P.A;
    Bw = P.B(:,widx);
    Bu = P.B(:,uidx);
    Cz = P.C(zidx,:);
    Cy = P.C(yidx,:);
    Dzw = P.D(zidx, widx); assert(~any(Dzw, 'all'), "Feedthrough from w to z cannot be non-zero. Closed-loop has infinite H2-norm.")
    Dzu = P.D(zidx, uidx);
    Dyw = P.D(yidx, widx);
    Dyu = P.D(yidx, uidx); assert(~any(Dyu, 'all'), "Feedthrough from u to y cannot be non-zero. Required transforms not implemented.")

    % Linearization variables
    % - Calling yalmip('clear') is upstream job
    X = sdpvar(nx);
    Y = sdpvar(nx);
    N = zeros(nx, ny);
    M = zeros(nu, nx);
    R = zeros(nu, ny);
    W = sdpvar(nw);
    gamma_sq = opts.hard_bound^2;
    if opts.minimize_gamma
        gamma_sq = sdpvar();
    end

    % LMIs / constraints
    cnst = [trace(W) <= gamma_sq;
            [(A*X + Bu*M) + (A*X + Bu*M)', (Cz*X + Dzu*M)';
             (Cz*X + Dzu*M), -eye(nz)] <= -tol * eye(nx+nz);
            [(Y*A + N*Cy)' + (Y*A + N*Cy), (Cz + Dzu*R*Cy)';
             (Cz + Dzu*R*Cy), -eye(nz)] <= -tol * eye(nx+nz);
            [W, (Bw + Bu*R*Dyw)', (Y*Bw + N*Dyw)';
             (Bw + Bu*R*Dyw), X, eye(nx);
             (Y*Bw + N*Dyw), eye(nx), Y] >= tol * eye(2*nx+nw)];

    if opts.enforce_strictly_proper && ~isequal(R, 0)
        cnst = [cnst;
                Dzu*R*Dyw == 0];  % enforce CL strictly proper
    end

    if isfinite(opts.variable_bound)
        cnst = [cnst;
                -opts.variable_bound <= allvariables(cnst) <= opts.variable_bound];
    end

    % solve
    if opts.minimize_gamma
        info.sol_diag = optimize(cnst, gamma_sq + nrm_cst_fctr * norm(allvariables(cnst)), opts.sdp_options);
    else
        info.sol_diag = optimize(cnst, [], opts.sdp_options);
    end

    info.sdp_vars = struct('X', value(X), 'Y', value(Y), 'M', value(N), 'N', value(N), 'R', value(R), 'W', value(W));
    gamma = sqrt(value(gamma_sq));


    % Recover controller
    Z = X*Y - eye(nx);
    J = Z \ ((A*X + Bu*M) + X*(A + Bu*R*Cy)' + X(Cz + Dzu*R*Cy)'*(Cz*X + Dzu*M)) / X;

    Dc = R;
    Cc = - Dc*Cy + M / X;
    Bc = Bu*Dc - Z \ (X*N - Bu*Dc);
    Ac = A + Bu*Dc*Cy + Bu*Cc - Bk*Cy + J;
    K = ss(Ac, Bc, Cc, Dc, TimeUnit=P.TimeUnit);


    if opts.enforce_strictly_proper, K.D = zeros(ny, nu); end  % gets rid of numerical noise
    CL = lft(P, K);

    info.constraints = cnst;
    info.X = Xcl;
    info.U = U;
    info.V = V;
end
