function Info = stab_OBP08_CDC_T3_yal(MSAi,partial_degs)
% function Info = stab_OBP08_CDC_T3_yal(MSAi,partial_degs)
% evaluates the robust stability of an uncertain time-varying linear system whose dynamic matrix
% is a homogeneous polynomial matrix with parameters belonging to the cartesian product of
% many simplexes, called multi-simplex. The test is performed through the existence of a
% homogeneous polynomial matrix with parameters in the multi-simplex. 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 condition is given in Theorem 3 of (Oliveira, Bliman &
% Peres, CDC08, to appear). The LMIs are programmed using YALMIP
% and can be solved by any LMI solver supported by YALMIP (SeDuMi is the default).
%
% inputs: MSAi         -> time-varying homogeneous polynomial matrix in the multi-simplex representation to
%                         be tested. (see the routine affine2MS_varying.m that converts an affine model
%                         to a multi-simplex model)
%         partial_degs -> partial degrees [g1 g2 ... gm] associated to the polynomially
%                         parameter-dependent Lyapunov matrix.
%
% outputs: Info.feas     -> stable (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.K        -> number of scalar variables
%          Info.L        -> number of LMI rows
%
% 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];
% 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);
% partial_degs = [1 0 1];
% Info = stab_OBP08_CDC_T3_yal(MSAi,partial_degs)
%
% Date: 18/02/2008
% Author: ricfow@dt.fee.unicamp.br

order = MSAi.nxm(1);
m = MSAi.m;

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

Info.cpusec_m = clock;

%new LMI system
LMIs = set([]);
%LMI rows counter
Info.L = 0;

%generate the exponents of the monomials to create the variables P_k
for i=1:m
    mon{i} = gen_coefs(MSAi.N(1,i),partial_degs(1,i))';
end
K_g = all_possible_comb(m,mon);
number_VAR = size(K_g,2);%number of variables
%generate the variables
for i=1:number_VAR
    mon_string = string_monomial(K_g(:,i)');
    eval(['P',mon_string,' = sdpvar(order,order,''symmetric'');']);
    P = eval(['P',mon_string]);
    LMIs = LMIs + set(P > 0);
    Info.L = Info.L + order;
end

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

for i=1:m
    mon{i} = gen_coefs(MSAi.N(1,i),partial_degs(1,i)+MSAi.deg(1,i))';%g_i+r_i
    mond{i} = 1:MSAi.bounds{i}.columns;
end
cartesian_dot = all_possible_comb(m,mond);
Kfr_g_plus_r = all_possible_comb(m,mon);
number_LMIs = size(Kfr_g_plus_r,2);

for i=1:m
    degnow = MSAi.deg;
    Kfr_r_plus_ep{i} = [];
    if partial_degs(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
        Kfr_r_plus_ep{i} = all_possible_comb(m,mon_ep);
    end
end


% A(alpha)'P(alpha) + P(alpha)A(alpha) + dot(P(alpha))< 0
for ell=1:size(cartesian_dot,2)
    dot_now = cartesian_dot(:,ell);
    for i=1:number_LMIs

        k = Kfr_g_plus_r(:,i);                
        % A(a)'P(a)+P(a)A(a) terms
        Term = zeros(order);
        for q=1:size(K_1,2)
            k_1 = K_1(:,q);
            if sum((k-k_1) >= 0) == size(k,1) %no negative component


                A = MSAi.coef{q}.coef_val;
                mon_string = string_monomial((k-k_1)');
                P = eval(['P',mon_string]);

                Term = Term + A'*P + P*A;                
            end
        end

        % dot{P(a)} terms
        Term_varying = zeros(order);
        for p=1:m
            for j=1:MSAi.N(1,p);
                epj = unitary_vector(MSAi,p,j);
                dot_P = zeros(order);
                add = 0;
                for r=1:size(Kfr_r_plus_ep{p},2)
                    kbar = Kfr_r_plus_ep{p}(:,r);
                    if sum( k-kbar >= 0) == size(k,1) %no negative component
                        kfinal = k-kbar+epj;
                        mon_string = string_monomial(kfinal');
                        P = eval(['P',mon_string]);
                        coef_1 = select_position_vector(MSAi,kfinal,p,j);
                        block  = select_block_vector(MSAi,kbar,p);                        
                        epji = unitary_vector_i(m,p);
                        coef_2 = prod(factorial(MSAi.deg+epji))/prod(factorial(kbar));
                        
                        dot_P = dot_P + coef_1*coef_2*P;
                        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
        Term = Term + Term_varying;
        LMIs = LMIs + set(Term < 0);
        Info.L = Info.L + order;        
    end
end

Info.cpusec_m = etime(clock,Info.cpusec_m);

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;
%capturing the solutions (if ones exist)
if sum(p < 0) == 0
    Info.Pi   = [];
    for i=1:number_VAR
        mon_string = string_monomial(K_g(:,i)');
        Info.Pi = [Info.Pi double(eval(['P',mon_string]))];
    end
    Info.feas = 1;
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 epj = unitary_vector_i(p,j)

epj = zeros(1,p);
for i=1:p
    if i == j
        epj(1,j) = 1;
    end    
end
