% H2SYN_LMI
% Synthesize a H2 (sub)optimal output-feedback controller using LMI methods.
%
% Implements LMI synthesis methods from Scherer & Weiland "LMIs in Control".
%
% 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_lmi(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

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

    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);
    nw = size(P.B, 2) - nu;
    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); Dzu = P.D(zidx, uidx);
    Dyw = P.D(yidx, widx); Dyu = P.D(yidx, uidx);
    assert(~any(Dzw, 'all'), "Feedthrough in w -> e not allowed. Closed-loop H2-norm not finite. Add artifical bandlimiting pole.");
    assert(~any(Dyu, 'all'), "Feedthrough in u -> y not allowed. Add bandlimiting artifical pole.");

    if opts.scale
        OPTS = struct(...
           'TOLREG', 0,...
           'LimitGain', false,...   % High gain may be required/desired (small R)
           'ScaleXUY', 1,...
           'Instrument', false);
        [Sx,Su,~,~] = rctutil.regScaleP12(A,Bw,Bu,Cz,Dzw,Dzu,0,Inf,OPTS);
        [~,Sy,~,~] = rctutil.regScaleP12(A',Cz',Cy',Bw',Dzw',Dyw',0,Inf,OPTS);
        % [Sx,Sy,~,~] = rctutil.regScaleP12(A',Cz',Cy',Bw',Dzw',Dyw',0,Inf,OPTS);

        A = Sx .* A ./ Sx';
        Bu = Sx .* Bu .* Su';
        Cy = Sy .* Cy ./ Sx';
        Dzu = Dzu .* Su';
        Dyw = Sy .* Dyw;

        % TODO:
        opts.minimum_decay = opts.minimum_decay * 1;
        opts.maximum_decay = opts.maximum_decay * 1;
        opts.minimum_damping = opts.minimum_damping * 1;
    end

    % Linearization variables
    % - Calling yalmip('clear') is upstream job
    X = sdpvar(nx);
    Y = sdpvar(nx);
    K = zeros(nx, nx);
    L = zeros(nx, ny);
    M = zeros(nu, nx);
    N = zeros(nu, ny);


    if ~opts.eliminate_variables
        K = sdpvar(nx, nx, 'full');
        L = sdpvar(nx, ny, 'full');
    end
    if ~opts.eliminate_variables || any(Dzu, 'all')
        M = sdpvar(nu, nx, 'full');
    end
    if (~opts.eliminate_variables || any(Dzu, 'all')) && ~opts.enforce_strictly_proper
        N = sdpvar(nu, ny, 'full');
    end

    Z = sdpvar(nz);
    gamma_sq = opts.hard_bound^2;
    if opts.minimize_gamma
        gamma_sq = sdpvar();
    end

    % Transformed closed-loop system
    X_ = [Y, eye(nx);
          eye(nx), X];
    A_ = [A*Y + Bu*M, A + Bu*N*Cy;
          K, X*A + L*Cy];
    B_ = [Bw + Bu*N*Dyw;
          X*Bw + L*Dyw];
    C_ = [Cz*Y + Dzu*M, Cz + Dzu*N*Cy];
    D_ = Dzu*N*Dyw;

    % LMIs / constraints
    cnst = [(trace(Z) <= gamma_sq):'H2 bound';
            ([X_, C_';
             C_, Z] >= tol * eye(2*nx+nz)):'H2 bound Schur'];
    if opts.enforce_strictly_proper && ~isequal(N, 0)
        cnst = [cnst;
                (D_ == 0):"strictly proper"];  % enforce CL strictly proper
    end

    %- Pole region constraint
    if opts.minimum_decay
        cnst = [cnst;
                (2*opts.minimum_decay * X_ + A_ + A_' <= -tol * eye(2*nx)):'min decay'];
    end
    if isfinite(opts.maximum_decay)
        cnst = [cnst;
                (-2*opts.maximum_decay * X_ - A_ - A_' <= -tol * eye(2*nx)):'max decay'];
    end
    if opts.minimum_damping
        cnst = [cnst;
                ([sqrt(1 - opts.minimum_damping^2) * (A_ + A_'), opts.minimum_damping * (A_ - A_');
                 -opts.minimum_damping * (A_ - A_'), sqrt(1 - opts.minimum_damping^2) * (A_ + A_')] <= -tol * eye(4*nx)):'min damping'];
    end
    if isfinite(opts.maximum_frequency)
        cnst = [cnst;
                ([-opts.maximum_frequency * X_, A_';
                  A_, -opts.maximum_frequency * X_] <= -tol * eye(4*nx)):'max freqeuncy'];
    end

    if opts.eliminate_variables  % eliminate at least K, L
        Psi = null([Cy, Dyw]);
        Phi_hat = [];
        if ~any(Dzu, 'all')  % eliminate also M, N
            Phi_hat = null(Bu);
            if isempty(Phi_hat), Phi_hat = []; end  % hotfix
        end
        tmp = blkdiag(Phi_hat, eye(nx+nw - size(Phi_hat, 1)));

        cnst = [cnst;
                Psi' * [A'*X + X*A, X*Bw;
                        Bw'*X, -eye(nw)] * Psi <= -tol * eye(size(Psi, 2));
                tmp' * [(A*Y + Bu*M) + (A*Y + Bu*M)', (Bw + Bu*N*Dyw);
                        (Bw + Bu*N*Dyw)', -eye(nw)] * tmp <= -tol * eye(size(tmp, 2))];
    else
        cnst = [cnst;
                ([A_' + A_,  B_;
                 B_', -eye(nw)] <= -tol * eye(2*nx+nw)):'Lyapunov stability'];
    end

    if opts.minimize_gamma && isfinite(opts.hard_bound)
        cnst = [cnst;
                (gamma_sq <= opts.hard_bound^2):'Gamma hard bound'];
    end

    if isfinite(opts.variable_bound)
        cnst = [cnst;
                (-opts.variable_bound <= allvariables(cnst) <= opts.variable_bound):'variable hard bound'];
    end
    if isfinite(opts.norm_bound)
        cnst = [cnst;
                (norm(X) <= opts.norm_bound):'norm bound'];
    end

    % solve for transformed solution
    if opts.minimize_gamma
        sol_diag = optimize(cnst, gamma_sq + nrm_cst_fctr * norm(allvariables(cnst)), opts.sdp_options);
    else
        sol_diag = optimize(cnst, norm(blkdiag(X, Y)), opts.sdp_options);
    end

    if opts.sdp_options.debug, check(cnst); end
    if opts.sdp_options.verbose, disp(sol_diag.info); end

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

    info.sol_diag = sol_diag;
    info.sdp_vars = struct('X', value(X), 'Y', value(Y), 'K', value(K), 'L', value(L), 'M', value(M), 'N', value(N), 'Z', value(Z));
    gamma = sqrt(value(gamma_sq));

    if reg_fctr > 1
        gamma_sq_relaxed = rlx_fctr * value(gamma_sq);
        alpha = sdpvar();

        cnst_bnd = [trace(Z) <= gamma_sq_relaxed;
                    cnst(2:end);
                    X <= alpha * eye(nx);
                    Y <= alpha * eye(nx)];
        if ~opts.eliminate_variables
            % TODO: what if just eliminating K, L
            cnst_bnd = [cnst_bnd;
                        [alpha * eye(nx+nu), [K, L; M, N];
                         [K, L; M, N]', alpha * eye(nx+ny)] >= 0];
        end

        info.diag_bnd = optimize(cnst_bnd, alpha, opts.sdp_options);
        info.cnst_bnd = cnst_bnd;
        info.alpha = value(alpha);

        alpha_relaxed = reg_fctr * value(alpha);
        beta = sdpvar();

        cnst_reg = [trace(Z) <= gamma_sq_relaxed;
                    cnst(2:end);
                    X <= alpha_relaxed * eye(nx);
                    Y <= alpha_relaxed * eye(nx);
                    [Y, beta*eye(nx);
                     beta*eye(nx), X] >= 0
                    beta >= 1];

        if ~opts.eliminate_variables
            cnst_reg = [cnst_reg;
                        [alpha_relaxed * eye(nx+nu), [K, L; M, N];
                         [K, L; M, N]', alpha_relaxed * eye(nx+ny)] >= 0];
        end
        info.diag_reg = optimize(cnst_reg, -beta, opts.sdp_options);
        info.cnst_reg = cnst_reg;
        info.beta = value(beta);

        gamma = sqrt(gamma_sq_relaxed);
    end

    % Recover controller
    [U, S, V] = svd(eye(nx) - value(X) * value(Y));
    %- Absorb S into U so that I - XY = U*V'
    %U = U*S; % This is bad if S disbalances U and V, better:
    U = U * sqrt(S);
    V = V * sqrt(S);


    % Alternative: use QR decomposition (but again Q and R may be disbalanced)
    % [Q, R] = qr(eye(nx) - value(X) * value(Y));
    % U = Q;
    % V = R';

    Xcl = [value(Y), V;
           eye(nx), zeros(nx)] \ [eye(nx), zeros(nx);
                                  value(X), U];

    % Note: we overwrite the linearization variable K, as we don't need it anymore after the next line.
    if ~opts.eliminate_variables
        K = [value(K) - value(X)*A*value(Y), value(L);
             value(M), value(N)];
        K = K / [V', zeros(nx, ny);
                 Cy * value(Y), eye(ny)];
        K = [U, value(X) * Bu;
             zeros(nu, nx), eye(nu)] \ K;

        K = ss(K(1:nx, 1:nx), K(1:nx, nx+1:nx+ny), K(nx+1:nx+nu, 1:nx), K(nx+1:nx+nu, nx+1:nx+ny), TimeUnit=P.TimeUnit);
    else
        Ac = sdpvar(nx, nx, 'full');
        Bc = sdpvar(nx, ny, 'full');
        Cc = sdpvar(nu, nx, 'full');
        Dc = sdpvar(nu, ny, 'full');

        Acl = [A + Bu*Dc*Cy, Bu*Cc;
               Bc*Cy, Ac];
        Bcl = [Bw + Bu*Dc*Dyw;
               Bc*Dyw];
        Ccl = [Cz + Dzu*Dc*Cy, Dzu*Cc];
        Dcl = [Dzu*Dc*Dyw];

        cnst_ctrl = [[Acl'*Xcl + Xcl*Acl, Xcl*Bcl;
                      Bcl'*Xcl, -eye(nw)] <= -tol];
        info.diag_ctrl = optimize(cnst_ctrl, [], opts.sdp_options);
        K = ss(value(Ac), value(Bc), value(Cc), value(Dc), TimeUnit=P.TimeUnit);
    end


    if opts.enforce_strictly_proper, K.D = zeros(ny, nu); end  % gets rid of numerical noise
    if opts.scale
        K.B = K.B .* Sy';
        K.C = Su .* K.C;
        K.D = Su .* K.D .* Sy';
    end
    CL = lft(P, K);

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