% H2SYN_SI Synthesize structured H2-suboptimal output-feedback controller using the sparsity invariance framework.
%
% 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_si(P, ny, nu, Knx, S, opts)
    % H2SYN_SI Synthesize a H2 optimal output-feedback controller with enforced information structure using LMI methods.
    %  Implements LMI synthesis methods from Schütte et al. 2021.
    %
    % Author: Maximilian Schütte <maximilian.schuette@desy.de>

    arguments
       P ss
       ny (1,1) {mustBeInteger,mustBePositive}
       nu (1,1) {mustBeInteger,mustBePositive}
       Knx (1,1) {mustBeInteger,mustBeNonnegative} % Number of controller states to be added
       S double {mustBeMember(S, [0 1])}
       opts (1,1) struct = h2syn_lmi_options()
    end

    mustBeNonnegative(opts.tolerance);
    tol = opts.tolerance;
    mustBeGreaterThanOrEqual(opts.relaxation_factor, 1);
    if opts.relaxation_factor > 1; warning("Relaxation not yet supported in this method. Ignoring..."); end
    mustBeGreaterThanOrEqual(opts.regularization_factor, 1);
    if opts.regularization_factor > 1; warning("Regularization not yet supported in this method. Ignoring..."); end
    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

    P = ss(P);
    nw = size(P.B, 2) - nu; widx = 1:nw; uidx = nw+1:nw+nu;
    nz = size(P.C, 1) - ny; zidx = 1:nz; yidx = nz+1:nz+ny;
    nx = order(P);
    Knu = ny + Knx; Kny = nu + Knx;

    A = P.A;
    Bu = P.B(:,uidx); Bw = P.B(:,widx);
    Cy = P.C(yidx, :); Cz = P.C(zidx, :);
    Dzu = P.D(zidx, uidx); Dzw = P.D(zidx, widx);
    Dyu = P.D(yidx, uidx); Dyw = P.D(yidx, widx);

    % Input checks
    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.");
        P = output_state_transform(prescale(P), 'e');
        P = truncate_coeff(P, 1e-12);
    end
    assert(~any(Dzw(:)), "Feedthrough in w -> e not allowed. Closed-loop H2-norm not finite. Add artifical bandlimiting pole.");
    assert(~any(Dyu(:)), "Feedthrough in u -> y not allowed. Add bandlimiting artifical pole.");
    assert(isequal(size(S), [Kny, Knu]), "Information structure imposed by S does not conform to system's IO structure.");

    % Augment plant
    A_ = blkdiag(A, zeros(Knx));
    Bw_ = [Bw; zeros(Knx, nw)];
    KBu = eye(Knx); % / sqrt(tunitconv('seconds', P.TimeUnit));
    Bu_ = blkdiag(Bu, KBu);
    Cz_ = [Cz, zeros(nz, Knx)];
    Cy_ = [Cy, zeros(ny, Knx)];
    Dzw_ = Dzw; Dzu_ = [Dzu, zeros(nz, Knx)];
    Dyw_ = Dyw; Dyu_ = [Dyu, zeros(ny, Knx)];
    nx_ = nx + Knx;

    S = [zeros(Kny, nx_-Knu), S];
    
    % Prepare
    % yalmip('clear'); % Don't clear user's variables
    info = struct('J', []);
    tol = opts.tolerance;

    % Optimization variables
    %- SI structure matrices
    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);
    Y = sparse(Ti, Tj, sdpvar(sum(T(:)), 1), Kny, nx_);
    clear Ti Tj;

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

    %- Slack variable
    Z = sdpvar(nz);

    % Objective
    gamma = opts.hard_bound;
    if opts.minimize_gamma
        gamma = sdpvar();
    end

    F = A_*X + Bu_*Y;

    % Constraints
    cnst = [([F + F', Bw_;
              Bw_', -gamma * eye(nw)] <= -tol * eye(nx_+nw)):'Lyapunov stability';
            ([X, X*Cz_' + Y'*Dzu_';
              Cz_*X + Dzu_*Y, Z] >= tol * eye(nx_+nz)):'H2 bound slack';
            (trace(Z) <= gamma):'H2 bound'];

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

    if opts.minimize_gamma && isfinite(opts.hard_bound)
        cnst = [cnst;
                (gammma <= opts.hard_bound):'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.variable_bound):'norm bound'];
    end

    % Solve!
    if opts.minimize_gamma
        sol_diag = optimize(cnst, gamma, opts.sdp_options);
    else
        sol_diag = optimize(cnst, norm(X), 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

    try
        K = value(Y) / value(X); % Recover controller.
        K = K(end-Kny+1:end,end-Knu+1:end);
        K(:, 1:ny) = K(:, 1:ny) ./ diag(Cy(:, end-ny+1:end))';
        K(1:Knx,:) = KBu * K(1:Knx, :);
        K = ss(K(nu+1:end, ny+1:end), K(nu+1:end,1:ny), K(1:nu,ny+1:end), K(1:nu, 1:ny), TimeUnit=P.TimeUnit);
        K = prescale(K);
    catch
        warning("Unable to recover controller, check solver status.");
        K = []; CL = []; gamma = Inf;
        return;
    end
    CL = lft(P, K);

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

    % Actual cost
    try
        P = lyap(CL.A, CL.B * CL.B');
        J = sqrt(trace(CL.C * P * CL.C'));

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

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

    info.J = J;
    info.sol_diag = sol_diag;
    info.constraints = cnst;
    info.sdp_vars = struct('X', value(X), 'Y', value(Y), 'Z', value(Z));
end
