function Info = stabMS_OBP09_ECC_T1_yal(MSAi,MSBi,deg_Lyap,deg_gain,param)
% function Info = stabMS_OBP09_ECC_T1_yal(MSAi,MSBi,deg_Lyap,deg_gain,param)
% synthesize a Lambda-homogeneous (parameters belonging to a multi-simplex) state feedback gain of
% arbitrary degree for a continuous time-varying linear system whose dynamic matrix and input matrix
% are Lambda-homogeneous polynomial matrix. 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 the LMI relaxation for Theorem 1 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
%         MSBi         -> Lambda-homogeneous polynomial input 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.scale  -> vector of real values used in the line search in beta (default = [1]).
%
%
% outputs: Info.feas     -> feasible (1) or not (0)
%          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
%
% 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];
% 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);
% MSB2i = affine2MS_LTV(B2theta,bounds_theta,bounds_dtheta);
% deg_Lyap = [1 0 1];
% deg_gain = [0 0 0]; %robust controller
% Info = stabMS_OBP09_ECC_T1_yal(MSAi,MSB2i,deg_Lyap,deg_gain)
% 
% Date: 18/02/2008
% Author: ricfow@dt.fee.unicamp.br

if nargin == 5
    if isfield(param,'scale')
        scale = param.scale;
    else
        scale = 1;
    end
else
    scale = 1;
end

mount_init = clock;
order = MSAi.nxm(1);
inputs = MSBi.nxm(2);
m = MSAi.m;
g = deg_Lyap;
s = deg_gain;
r = MSAi.deg;

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

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))';
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);

%generate the variables
for i=1:size(K_g,2)
    mon_string = string_monomial(K_g(:,i)');
    eval(['W',mon_string,' = sdpvar(order,order,''symmetric'');']);
    P = eval(['W',mon_string]);
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,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

mount_init = etime(clock,mount_init);
for v=1:size(scale,2)

    Info.cpusec_m = clock;
    %new LMI system
    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

    % [A(a)F(a)+F(a)'A(a)'+B(a)Z(a)+Z(a)'B(a)'+dot{P}(a)      *    ]
    % [P(a)-F(a)+F(a)'A(a)'+Z(a)'B(a)'                   F(a)+F(a)'] < 0

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

            %Term 1 ( A(a)F(a) + B(a)Z(a)  )
            Term_1 = zeros(order);
            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;
                            B = MSBi.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+B*Z);
                        end
                    end
                end
            end
            %Term 2 (  F(a)  )
            Term_2 = zeros(order);
            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 (  P(a)  )
            Term_3 = zeros(order);
            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;
                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';
            T22 = -scale(1,v)*(Term_2 + Term_2');

            LMIs = LMIs + set([T11 T21';T21 T22] < 0);
            Info.L = Info.L + 2*order;
        end
    end

    Info.cpusec_m = etime(clock,Info.cpusec_m);
    Info.cpusec_m = Info.cpusec_m + mount_init;
    Info.K = size(getvariables(LMIs),2);

    sol = solvesdp(LMIs,[],sdpsettings('verbose',0,'solver','sedumi'));
    Info.cpusec = sol.solvertime;
    [p,d]=checkset(LMIs);

    Info.feas = 0;
    Info.beta=  0;
    %capturing the solutions (if ones exist)
    if sum(p < 0) == 0
        Info.Wi = [];
        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.Z{i}.val = double(eval(['Z',mon_string]));
            Info.Z{i}.mon = K_s(:,i)';
            Info.F{i}.val = double(eval(['F',mon_string]));
            Info.F{i}.mon = K_s(:,i)';
        end
        if size(K_s,2) == 1 % determine the closed-loop matrix if the gain is robust
            Info.MSAcl = MSAi;
            for i=1:MSAi.nmon
                Info.MSAcl.coef{i}.coef_val = MSAi.coef{i}.coef_val + MSBi.coef{i}.coef_val*Info.Z{1}.val*inv(Info.F{1}.val);
            end
        end
        Info.feas = 1;
        Info.beta = scale(1,v);
        break;
    end
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));
