function Info = synth_tfs_TOP11b_T2_yal(Ai,Bi,param)
%function Info = stab_fuzzy_msimplex_yal(deg);
% inputs: Ai            -> Ai = [A1 A2 ... AN]
%         Bi            -> Bi = [B1 B2 ... BN]
%         partial_degs  -> ex. deg = [g1 ... gm];
%         param.phi     -> bnd rate limit ex. h = [0.85;0.85];
%         param.m       -> Number of simplexes
%         param.r       -> Length of each simplex, ex. r = [2 2 ... 2];

% 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
%
% Date: 12/01/2010
% Author: edutog@dt.fee.unicamp.br

[order,vertices] = size(Ai);
vertices = vertices / order;
inputs = size(Bi,2)/vertices;

for iq = 1:vertices
        A{iq}=Ai(:,(iq-1)*order+1:iq*order);
        B{iq}=Bi(:,(iq-1)*inputs+1:iq*inputs);        
end

%-------------
% Parameters
%-------------
deg_Lyap = param.deg_Lyap;
deg_gain = param.deg_gain;
m = param.m; %Number of simplexes
h = param.phi; %phi of each element of all simplexes

MSAi.m = m;
MSAi.nxm = [order order];
MSBi.nxm = [order inputs];

%Individual parameters
sigma = 1; MSAi.deg=[];
r = param.r; %Length of each simplex
for i=1:MSAi.m
    MSAi.N(1,i) = r(i); 
    MSAi.deg = [MSAi.deg sigma]; 
end


%gerar indices para A_{i_1_...i_n}
for i=1:m
    mon_sigma{i} = gen_coefs(r(i),sigma)'; %(vertices,deg)
end
MSAi.mon = all_possible_comb(m,mon_sigma);
MSAi.nmon = size(MSAi.mon,2);

% Vertices
for i=1:vertices
    MSAi.coef{i}.coef_val = A{i};
    MSBi.coef{i}.coef_val = B{i};
end


% Time-derivative bounds
for i=1:m
    phi_vet = [];
    for j = 1:r(i)
       phi_vet = [phi_vet; -h(i,j) h(i,j)];
    end
    MSAi.bounds{i}.H = gen_base(r(i),phi_vet);
    MSAi.bounds{i}.columns = r(i);
end

% Call stabilizability theorem
Info = synth_TOP11b_T2_yal(MSAi,MSBi,deg_Lyap,deg_gain,param);




%__________________________________________________________________________
function Info = synth_TOP11b_T2_yal(MSAi,MSBi,deg_Lyap,deg_gain,param)
%
% Date: 12/01/2010
% Author: edutog@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);
							
							epji = unitary_vector_i(m,p);
                            coef_2 = factorial(w(1,p)-g(1,p)+epji)/prod(factorial(kbar));

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

%________________________________________________________________________________________________________
function epj = unitary_vector_i(p,j)
epj = zeros(1,p);
for i=1:p
    if i == j
        epj(1,j) = 1;
    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 [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 Coefs = nomial(n,n2,v,v2,var,Coefs)

if v2 < v
    for i=0:n2
        var(v2+1) = i;
        Coefs = nomial(n, n2-i, v, v2+1, var,Coefs);
    end
else
    var(v+1)=n2;
    prod = part(n,var,v);
    este = [];
    for j=1:v
        este = [este var(j+1)];
    end
    Coefs = [Coefs;este];
end

%________________________________________________________________________________________________________
% function [string] = string_monomial(monomial)
% 
% string = '';
% for i=1:size(monomial,2)
%     string = strcat(string,'_',int2str(monomial(1,i)));
% end

%________________________________________________________________________________________________________
function [H] = gen_base(N,bounds)
%function [H] = gen_base(N,bounds)
% This function generates the representation base H  used by the diffential
% inclusion method
% inputs: N       -> number of vertices of the polytope
%         bounds  -> time-derivative bounds. ex.: [[-1 1];[-1 1]...[-1 1]];
%
% output: H -> representation base

H = solve_plane(N,bounds,0);

%________________________________________________________________________________________________________
function H = solve_plane(N,limites,plano)

H = [];
for i=1:N
    for k=1:N
        if i == k
            continue;
        end
        sobra = [];
        pos = [];
        if N == 2
            if -limites(i,1) >= limites(k,1) & -limites(i,1) <= limites(k,2)
                vec_final(1,i) = limites(i,1);
                vec_final(1,k) = -limites(i,1);
                if ja_contem(H,vec_final') == 0
                    H = [H vec_final'];
                end
            end
            if -limites(i,2) >= limites(k,1) & -limites(i,2) <= limites(k,2)
                vec_final(1,i) = limites(i,2);
                vec_final(1,k) = -limites(i,2);
                if ja_contem(H,vec_final') == 0
                    H = [H vec_final'];
                end
            end
        else
            for o=1:N
                if o ~= i & o ~= k
                    pos = o;
                    sobra = limites(o,:);
                    break;
                end
            end
            for m=1:size(sobra',1);
                soma1 = -sum(sobra(:,m)) - limites(i,1) - plano;
                soma2 = -sum(sobra(:,m)) - limites(i,2) - plano;
                if soma1 >= limites(k,1) & soma1 <= limites(k,2)
                    vec_final(1,i) = limites(i,1);
                    vec_final(1,k) = soma1;
                    vec_final(1,pos) = sobra(:,m);

                    if ja_contem(H,vec_final') == 0
                        H = [H vec_final'];
                    end
                end
                if soma2 >= limites(k,1) & soma2 <= limites(k,2)
                    vec_final(1,i) = limites(i,2);
                    vec_final(1,k) = soma2;
                    vec_final(1,pos) = sobra(:,m);
                    if ja_contem(H,vec_final') == 0
                        H = [H vec_final'];
                    end
                end
            end
        end
    end
end

%________________________________________________________________________________________________________
function R = ja_contem(H,h)

R = 0;
for i=1:size(H,2)
    if sum(H(:,i) == h) == size(h,1)
        R = 1;
        return;
    end
end

