function Info = h2MS_OBP09_ECC_T2_yal(MSAi,MSB1i,MSB2i,MSCi,MSD2i,deg_Lyap,deg_gain,param)
% function Info = h2MS_OBP09_ECC_T2_yal(MSAi,MSB1i,MSB2i,MSCi,MSD2i,deg_Lyap,deg_gain,param)
% synthesize a H2 Lambda-homogeneous (parameters belonging to a multi-simplex) state feedback gain of
% arbitrary degree for a continuous time-varying linear system whose system matrices
% are Lambda-homogeneous polynomial matrices. The design condition is performed through the existence of a
% Lambda-homogeneous polynomial Lyapunov matrix. The parameters can  be: (i) time-invariant;
% (ii) time-varying with bounded rates of variation; (iii) % time-varying with unknown time
%variation. The partial degrees of the Lyapunov matrix, associated to each simplex inside the
% multi-simplex, can be freely chosen by the user except in the case where nothing is known about
% the parameter variation. In this case, the associated degree must be zero (quadratic stability).
% The degrees of the gains are chosen independently of the degrees of the Lyapunov matrix.
% The condition is given in Theorem 2 of (Oliveira, Bliman &
% Peres, ECC09, submitted for publication). The LMIs are programmed using YALMIP
% and can be solved by any LMI solver supported by YALMIP (SeDuMi is the default).
%
% inputs: MSAi         -> Lambda-homogeneous polynomial dynamic matrix
%         MSB1i        -> Lambda-homogeneous polynomial disturbance input matrix
%         MSB2i        -> Lambda-homogeneous polynomial control input matrix
%         MSCi         -> Lambda-homogeneous polynomial output matrix
%         MSD2i        -> Lambda-homogeneous polynomial control output matrix
%         deg_Lyap     -> partial degrees [g1 g2 ... gm] associated to the Lambda-homogeneous
%                         Lyapunov matrix.
%         deg_gain     -> partial degrees [s1 s2 ... sm] associated to the Lambda-homogeneous
%                         synthesized gains.
%         param        -> optional parameters
%         param.h2     -> if this parameter is different of zero, the routine will
%                         check the feasibility of the LMIs for this value of the H-2 norm.
%         param.scale  -> vector of real values used in the line search in beta (default = [1]).
%
%
% outputs: Info.h2       ->  H-2 guaranteed cost (0 if unfeasible)
%          Info.cpusec   -> cpu time to solve the LMIs (seconds)
%          Info.cpusec_m -> cpu time to mount the LMIs (seconds)
%          Info.Pi       -> Solution variables Pk
%          Info.Zi       -> synthesizes matrices used to construct the rational Lambda-homogeneous gain
%          Info.Fi       -> synthesizes matrices used to construct the rational Lambda-homogeneous gain
%          Info.K        -> number of scalar variables
%          Info.L        -> number of LMI rows
%          Info.beta     -> value of beta used in the line search
%          Info.costs    -> intermediate costs obtained in the line search.
%
%
% Example: affine model A(theta)=A0 + A_1theta_1(t) + A_2theta_2(t) + A_3theta_3, with
% A0 = [-2     1; -1    -3]; A1 = [2     1; 0    -1]; A2 = [1     1;-1     1 ]; A3 = [0     0; 0    -1];
% Atheta = [A0 A1 A2 A3]; 
% B20 = [1; 0]; B21=[1;0]; B22 = [0;0]; B23 = [0;0]; 
% B2theta = [B20 B21 B22 B23];
% B10 = [1; 0]; B11=[0;0]; B12 = [0;0];  B13 = [0;0];
% B1theta = [B10 B11 B12 B13];
% C0 = [1 0]; C1 = [0 0]; C2 = [0 0]; C3=[0 0];
% Ctheta = [C0 C1 C2 C3];
% D20 = [0.0]; D21=[0]; D22 = [0];  D23 = [0];
% D2theta = [D20 D21 D22 D23];
% bounds_theta = [-1 1;-1 1;-1 1];
% bounds_dtheta = [-1.5 1.5;-inf inf;0 0];
% MSAi  = affine2MS_LTV(Atheta ,bounds_theta,bounds_dtheta);
% MSB1i = affine2MS_LTV(B1theta,bounds_theta,bounds_dtheta);
% MSB2i = affine2MS_LTV(B2theta,bounds_theta,bounds_dtheta);
% MSCi  = affine2MS_LTV(Ctheta ,bounds_theta,bounds_dtheta);
% MSD2i = affine2MS_LTV(D2theta,bounds_theta,bounds_dtheta);
% deg_Lyap = [1 0 1];
% deg_gain = [0 0 0]; %robust controller
% param.scale = 0.001; %tunning parameter
% Info = h2MS_OBP09_ECC_T2_yal(MSAi,MSB1i,MSB2i,MSCi,MSD2i,deg_Lyap,deg_gain,param)
% 
% Date: 15/09/2008
% Author: ricfow@dt.fee.unicamp.br

order    = MSAi.nxm(1);
inputs_d = MSB1i.nxm(2);
inputs_c = MSB2i.nxm(2);
outputs  = MSCi.nxm(1);


if nargin == 8
    if isfield(param,'h2')
        h2 = param.h2;
    else
        h2 = 0;
    end
    if isfield(param,'scale')
        scale = param.scale;
    else
        scale = 1;
    end
    if isfield(param,'precision')
        precision = param.precision;
    else
        precision= 1e-7;
    end    
else
    h2 = 0;
    scale = 1;
    precision = 1e-7;
end

m = MSAi.m;
g = deg_Lyap;
s = deg_gain;
r = MSAi.deg;

if check_partial_degrees(MSAi,g) == 0
    Info = [];
    return;
end

mount_init = clock;

w = max([g;r+s]);

%generate the exponents of the monomials to create the variables P_k
for i=1:m
    mon_g{i} = gen_coefs(MSAi.N(1,i),g(i))';
    mon_s{i} = gen_coefs(MSAi.N(1,i),s(i))';
    mon_w{i} = gen_coefs(MSAi.N(1,i),w(i))';
    mon_w_r_s{i} = gen_coefs(MSAi.N(1,i),w(i)-r(i)-s(i))';
    mon_w_s{i} = gen_coefs(MSAi.N(1,i),w(i)-s(i))';
    mon_w_g{i} = gen_coefs(MSAi.N(1,i),w(i)-g(i))';
    mon_w_r{i} = gen_coefs(MSAi.N(1,i),w(i)-r(i))';
end
K_g = all_possible_comb(m,mon_g);
K_s = all_possible_comb(m,mon_s);
K_w = all_possible_comb(m,mon_w);
K_w_r_s = all_possible_comb(m,mon_w_r_s);
K_w_s = all_possible_comb(m,mon_w_s);
K_w_g = all_possible_comb(m,mon_w_g);
K_w_r = all_possible_comb(m,mon_w_r);

%generate the variables
if h2== 0
    gamma = sdpvar(1,1);
    obj = gamma;
else
    gamma = h2^2;
    obj = [];
end

for i=1:size(K_g,2)
    mon_string = string_monomial(K_g(:,i)');
    eval(['W',mon_string,' = sdpvar(order,order,''symmetric'');']);
    eval(['T',mon_string,' = sdpvar(outputs,outputs,''symmetric'');']);
end

for i=1:size(K_s,2)
    mon_string = string_monomial(K_s(:,i)');
    eval(['F',mon_string,' = sdpvar(order,order,''full'');']);
    eval(['Z',mon_string,' = sdpvar(inputs_c,order,''full'');']);
end

K_r = MSAi.mon;
size_K_r = size(K_r,2);%number of monomials in the matrix to be tested

for i=1:m
    mond{i} = 1:MSAi.bounds{i}.columns;
end
cartesian_dot = all_possible_comb(m,mond);
number_LMIs = size(K_w,2);

for i=1:m
    degnow = w-g;
    K_w_min_g_plus_ep{i} = [];
    if g(1,i) >= 1
        degnow(1,i) = degnow(1,i) + 1;
        for j=1:m
            mon_ep{j} = gen_coefs(MSAi.N(1,j),degnow(1,j))';
        end
        K_w_min_g_plus_ep{i} = all_possible_comb(m,mon_ep);
    end
end


% [A(a)F(a)+F(a)'A(a)'+B2(a)Z(a)+Z(a)'B2(a)'+dot{P}(a)      *      * ]
% [W(a)-F(a)+F(a)'A(a)'+Z(a)'B2(a)'                   F(a)+F(a)'   * ] < 0
% [B1(a)'                                                0        -I ]
%
% [ F(a)+F(a)'-W(a)       *  ]
%   C(a)F(a)+D2(a)Z(a)   T(a)]  > 0
%
% trace(T(a)) < gamma
%
mount_init = etime(clock,mount_init);
times_m = [];
times_s = [];
Info.costs = [];
rr=  1;
for v=1:size(scale,2)
    solve_ini = clock;
    LMIs = set([]);    
    
    %LMI rows counter
    Info.L = 0;
    for i=1:size(K_g,2)
        mon_string = string_monomial(K_g(:,i)');
        W = eval(['W',mon_string]);
        LMIs = LMIs + set(W > 0);        
        Info.L = Info.L + order;
    end

    for ell=1:size(cartesian_dot,2)
        dot_now = cartesian_dot(:,ell);
        for i=1:number_LMIs
            k = K_w(:,i);

            Term_1 = zeros(order); %Term 1 ( A(a)F(a) + B(a)Z(a)  )
            Term_4 = zeros(order,outputs); % Term 4 ( F(a)'C(a)'+Z(a)'D2(a)' )
            for t1=1:size(K_w_r_s,2)
                ktilde = K_w_r_s(:,t1);
                if sum((k-ktilde) >= 0) == size(k,1) %no negative component
                    for tr=1:size(K_r,2)
                        kbar = K_r(:,tr);
                        if sum((k-ktilde-kbar) >= 0) == size(k,1) %no negative component
                            c = prod(factorial(w-r-s))/prod(factorial(ktilde));
                            A  = MSAi.coef{tr}.coef_val;
                            B2 = MSB2i.coef{tr}.coef_val;
                            C  = MSCi.coef{tr}.coef_val;
                            D2 = MSD2i.coef{tr}.coef_val;
                            mon_string = string_monomial((k-ktilde-kbar)');
                            F = eval(['F',mon_string]);
                            Z = eval(['Z',mon_string]);
                            Term_1 = Term_1 + c*(A*F+B2*Z);
                            Term_4 = Term_4 + c*(F'*C'+Z'*D2');
                        end
                    end
                end
            end
            Term_2 = zeros(order); %Term 2 (  F(a)  )
            for t2=1:size(K_w_s,2)
                ktilde = K_w_s(:,t2);
                if sum((k-ktilde) >= 0) == size(k,1)
                    c = prod(factorial(w-s))/prod(factorial(ktilde));
                    mon_string = string_monomial((k-ktilde)');
                    F = eval(['F',mon_string]);
                    Term_2 = Term_2 + c*F;
                end
            end

            Term_3 = zeros(order); %Term 3 (  P(a)  )
            Term_8 = zeros(outputs); %Term 8 (  W(a)  )
            for t3=1:size(K_w_g,2)
                ktilde = K_w_g(:,t3);
                if sum((k-ktilde) >= 0) == size(k,1)
                    c = prod(factorial(w-g))/prod(factorial(ktilde));
                    mon_string = string_monomial((k-ktilde)');
                    W = eval(['W',mon_string]);
                    Term_3 = Term_3 + c*W;
                    T = eval(['T',mon_string]);
                    Term_8 = Term_8 + c*T;
                end
            end

            Term_5 = zeros(MSB1i.nxm); % Term 5 ( B1(a) )
            Term_7 = zeros(1); % Term 7 (   I   )

            for t4=1:size(K_w_r,2)
                ktilde = K_w_r(:,t4);
                if sum((k-ktilde) >= 0) == size(k,1)
                    c = prod(factorial(w-r))/prod(factorial(ktilde));
                    B1 = get_coefficient(MSB1i,k-ktilde);
                    Term_5 = Term_5 + c*B1;
                    Term_7 = Term_7 + c;
                end
            end

            Term_varying = zeros(order);% (  dot{P}(a)  )
            for p=1:m
                for j=1:MSAi.N(1,p);
                    epj = unitary_vector(MSAi,p,j);
                    dot_P = zeros(order);
                    add = 0;
                    for q=1:size(K_w_min_g_plus_ep{p},2)
                        kbar = K_w_min_g_plus_ep{p}(:,q);
                        if sum( k-kbar >= 0) == size(k,1) %no negative component
                            kfinal = k-kbar+epj;
                            mon_string = string_monomial(kfinal');
                            W = eval(['W',mon_string]);
                            coef_1 = select_position_vector(MSAi,kfinal,p,j);
                            block = select_block_vector(MSAi,kbar,p);
                            coef_2 = factorial(w(1,p)-g(1,p)+1)/Pi(block');

                            dot_P = dot_P + coef_1*coef_2*W;
                            add = 1;                            
                        end
                    end
                    pos_col = cartesian_dot(p,ell);
                    if add == 1
                        Term_varying = Term_varying + dot_P*MSAi.bounds{p}.H(j,pos_col);
                    end                    
                end
            end
            T11 = Term_1+Term_1'+Term_varying;
            T21 = Term_3-Term_2+scale(1,v)*Term_1';
            T31 = Term_5';
            T22 = -scale(1,v)*(Term_2 + Term_2');
            T32 = zeros(inputs_d,order);
            T33 = -eye(inputs_d)*Term_7;            
            LMIs  = LMIs + set([T11 T21' T31';
                T21 T22  T32';
                T31 T32  T33] < 0);
          

            T11 = Term_2 + Term_2'-Term_3;
            T21 = Term_4';
            T22 = Term_8;
            LMIs  = LMIs + set([T11 T21';
                T21 T22] > 0);

            LMIs = LMIs + set(trace(Term_8) < Term_7*gamma);
            Info.L = Info.L + 3*order+inputs_d+outputs+1;
        end
    end

    times_m = [times_m (etime(clock,solve_ini)+mount_init)];    

    Info.K = size(getvariables(LMIs),2);
    sol = solvesdp(LMIs,obj,sdpsettings('verbose',0,'solver','sedumi'));
    
    times_s = [times_s sol.solvertime];
    
    p=min(checkset(LMIs));
    Info.delta = p;
    if p > 0
        p = 0;
    end

    h2 = 0;
    %capturing the solutions (if ones exist)
    if abs(p) < precision %adopted precision for the minimum primal residual
        Info.Wi = [];
        Info.Zi = [];
        Info.Fi = [];
        for i=1:size(K_g,2)
            mon_string = string_monomial(K_g(:,i)');
            Info.Wi = [Info.Wi double(eval(['W',mon_string]))];            
        end
        for i=1:size(K_s,2)
            mon_string = string_monomial(K_s(:,i)');
            Info.Zi = [Info.Zi double(eval(['Z',mon_string]))];
            Info.Fi = [Info.Fi double(eval(['F',mon_string]))];
        end
        h2 = sqrt(double(gamma));               
    end
    Info.costs = [Info.costs h2];
end
[x,y] = find(Info.costs > 0);
if isempty(y)
    Info.h2 = 0;
    Info.cpusec = 0;
    Info.cpusec_m = mount_init;
    Info.beta = 0;
else
    [w,z]=min(Info.costs(y));
    ind = y(z);
    Info.h2 = w;
    Info.beta = scale(1,ind);
    Info.cpusec_m = times_m(1,ind);
    Info.cpusec = times_s(1,ind);    
end

%________________________________________________________________________________________________________
function epj = unitary_vector(MSAi,p,j)

epj = [];
for i=1:MSAi.m
    now = zeros(MSAi.N(1,i),1);
    if i == p
        now(j,1) = 1;
    end
    epj = [epj; now];
end

%________________________________________________________________________________________________________
function elem = select_position_vector(MSAi,vector,p,j)

pos = 1;
for i=1:MSAi.m
    if i == p
        elem = vector(pos+j-1,1);
    end
    pos = pos + MSAi.N(1,i);
end

%________________________________________________________________________________________________________
function block = select_block_vector(MSAi,vector,p)

pos = 1;
for i=1:MSAi.m
    if i == p
        block = vector(pos:pos+MSAi.N(1,i)-1,1);
        break;
    end
    pos = pos + MSAi.N(1,i);
end

%________________________________________________________________________________________________________
function [comb] = gen_coefs(vertices,degree)
%generates all the solutions of: a_1 + a_2 + ... + a_vertices = degree

name = strcat('comb_N',int2str(vertices),'d',int2str(degree),'.mat');
if exist(name,'file') == 2
    load(name);
else
    comb = [];
    if vertices == 1
        comb = degree;
    else
        for i = 0:degree
            temp = gen_coefs(vertices-1,degree-i);
            comb   = [ comb
                i*ones(size(temp,1),1) temp ];
        end
    end
end

%________________________________________________________________________________________________________
function Resp = all_possible_comb(n,H)

var = [];
Resp = call_recur(n,H,1,var,[],1);

function Resp = call_recur(n,H,na,var,Resp,pos_ini)

if na <= n
    tam = size(H{na},1);
    for i=1:size(H{na},2)
        var(pos_ini:pos_ini+tam-1,1) = H{na}(:,i);
        Resp = call_recur(n,H,na+1,var,Resp,pos_ini+tam);
    end
else
    Resp = [Resp var];
end

%________________________________________________________________________________________________________
function [string] = string_monomial(monomial)

string = '';
for i=1:size(monomial,2)
    string = strcat(string,'_',int2str(monomial(1,i)));
end

%________________________________________________________________________________________________________
function good = check_partial_degrees(MSAi,partial_degs)

good = 1;
if ~isfield(MSAi,'bounds')
    fprintf('The time-derivative bounds structure (field ''bounds'') was not given !\n');
    good = 0;
    return;
end
for i=1:MSAi.m
    if MSAi.bounds{i}.columns == 1
        if MSAi.bounds{i}.H(1,1) == Inf
            if partial_degs(1,i) > 0
                fprintf('The partial degree associated to the %d-th simplex inside the multi-simplex only \n can be zero (quadratic stability) since the parameter is arbitrarily time-varying !\n',i);
                good = 0;
                return;
            end
        end
    end
    if partial_degs(1,i) < 0 | rem(partial_degs(1,i)*10,10) ~= 0
        fprintf('The %d-th partial degree must be an nonnegative integer !\n',i);
        good = 0;
        return;
    end
end

%________________________________________________________________________________________________________
function R = Pi(monomial)

R=prod(factorial(monomial));

%________________________________________________________________________________________________________
function value = get_coefficient(all_coefs,mon)

for i=1:size(all_coefs.mon,2)
    if sum(all_coefs.mon(:,i) == mon) == size(mon,1)
        value = all_coefs.coef{i}.coef_val;
        return;
    end
end

