% H2SYN_SI2 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, gamma, J, sol_diag] = h2syn_si2(P, S, epsilon, options)
    arguments
       P ss
       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} = 0
    end

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

    nx = order(P);
    widx = startsWith(P.InputName, 'w'); nw = sum(widx);
    uidx = startsWith(P.InputName, 'u'); nu = sum(uidx);
    zidx = startsWith(P.OutputName, 'z'); nz = sum(zidx);
    yidx = startsWith(P.OutputName, 'e'); ny = sum(yidx);

    Cy = P.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.");
        P = balreal(P);
        P = output_state_transform2(P, 'e');
        P = truncate_coeff(P, 1e-20);
        P = prescale(P);
    end

    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); Duw = P.D(yidx, widx);

    % Verify
    assert(~any(Dzw(:)), "H2-performance not finite with feedthrough from w -> z");
    assert(~any(Duw(:)), "Feedthrough in w -> e not allowed, add artifical pole");
    assert(~any(Dyu(:)), "Feedthrough in u -> y not allowed, add artifical pole");

    assert(isequal(size(S), [nu, ny]));
    S = [zeros(nu, nx-ny), S];

    % Optimization Variables
    %- Sparsity Invariance 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), nu, 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;

    %- Z variable
    Z = sdpvar(nw);

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

    % Objective
    obj = trace(Z);

    % Constraints
    cnst = [X >= epsilon * eye(nx);
            [(A*X + Bu*Y) + (A*X + Bu*Y)', X*Cz';
             Cz*X, -eye(nz)] <= -epsilon * eye(nx+nz);
            [X, Bw;
             Bw', Z] >= 0];

    % 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!");
    end

    try
        K = value(Y) / value(X); % Recover controller.
        K = K(end-ny+1:end, end-ny+1:end);
        K = K / 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

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

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

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