function output = ssf_stabTS_MOP09_T2_yal(Ai,Bi,deg,d,param)
% function output = ssf_stabTS_MOP09_T2_yal(Ai,Bi,deg,d)
%
% Synthesize a linear parameter-dependent state feedback control gain that ensure closed-loop quadratic 
% stability for continuous-time Takagi-Sugeno fuzzy systems. Polya's relaxations are used to guarantee
% convergence. Tha slack variables can be polynomially of arbitrary degree, improving the rate of
% convergence. The condition was published in Theorem 2 of (Montagner et at.,
% 2009, IEEE-TFS). The LMIs are programmed using YALMIP  and can be solved  by any LMI solver supported by
% YALMIP (SeDuMi is the default).
%
% input: Ai,Bi         -> vertices of the polytopes (Ai = [A1 A2 ... AN], Bi = [B1 B2 ... BN])
%        deg           -> degree of the polynomially parameter-dependent slack variable X(alpha)
%        d             -> number of Polya's relaxations
%
% output:output.feas     -> 1 if such gain exits, 0 if unfeasible
%        output.cpusec   -> cpu time to solve the LMIs (seconds)
%        output.cpusec_m -> cpu time to mount the LMIs (seconds)
%        output.K        -> number of scalar variables
%        output.L        -> number of LMIs rows
%        output.W        -> solution variable W (Lyapunov matrix)
%        output.Ki       -> synthesized gains ([Z_1 Z_2 .. Z_N]*inv(W)]
%
% Example: 2 states and 2 vertices
%
% A1 = [-0.9  0.2;
%       -0.5  -1.9 ];
% A2 = [-1.1  -0.1;
%       0.5    -2.8];
% B1 = [1;0];  B2 = [0;-1];
% Ai = [A1 A2]; Bi = [B1 B2];
% output = ssf_stabTS_MOP09_T2_yal(Ai,Bi,1,0)
% 
% Date: 27/08/2007
% Author: ricfow@dt.fee.unicamp.br

if nargin == 5
    if isfield(param,'normalize_d')
        % the coefficients of the LMIs will be normalized, avoiding numerical problems
        % when d grows larger.
        normalize_d = param.normalize_d;
    else
        normalize_d = 1;
    end
    if isfield(param,'normalize_g')
        % the coefficients of the LMIs will be normalized, avoiding numerical problems
        % when g grows larger.
        normalize_g = param.normalize_g;
    else
        normalize_g = 1;
    end
else
    normalize_d = 1;
    normalize_g = 1;
end

%determine the number of vertices of polytope
[order,vertices] = size(Ai);
vertices = vertices / order;
[order,inputs] = size(Bi);
inputs = inputs / vertices;

output.cpusec_m = clock;

%new LMI system
LMIs = set([]);

output.cpusec_m = clock;

%new LMI system
LMIs = set([]);

% the coefficients of the LMIs will be normalized, avoiding numerical problems when d grows larger.
normalized = 1;

%generate the exponents of the monomials to create the variables P_k
K_g = gen_coefs(vertices,deg);
number_VAR = size(K_g,1);%number of variables

%generate the variables
for i=1:number_VAR
    mon_string = string_monomial(K_g(i,:));
    eval(['X',mon_string,' = sdpvar(2*order,order,''full'');']);
end

for i=1:vertices
    Z{i} = sdpvar(inputs,order,'full');
end

W = sdpvar(order,order,'symmetric');
LMIs = LMIs + set(W > 0);

Kfr_g_d = gen_coefs(vertices,deg + d);
Kfr_d = gen_coefs(vertices,d);

number_LMIs = size(Kfr_g_d,1);
size_Kfr_d = size(Kfr_d,1);

Kfr_g_d_1 = gen_coefs(vertices,deg + d + 1);
number_LMIs = size(Kfr_g_d_1,1);

for ell=1:number_LMIs

    k = Kfr_g_d_1(ell,:);

    Term = zeros(2*order);
    if normalized == 1
        max_coef = max_coefficient(k,size_Kfr_d,Kfr_d,d,vertices);
    else
        max_coef = 1;
    end
    for m=1:size_Kfr_d
        kprime = Kfr_d(m,:);
        if size(find(k-kprime>=0),2) == vertices
            Cfr  = factorial(d)/Pi(kprime)/max_coef;
            for i=1:vertices
                if k(i) > kprime(i)
                    e = zeros(1,vertices);
                    e(i) = 1;

                    A = Ai(:,(i-1)*order+1:i*order);
                    Bfr = [A' -eye(order)];
                    mon_string = string_monomial(k-kprime-e);
                    X = eval(['X',mon_string]);
                    Term = Term + Cfr*(X*Bfr + Bfr'*X');
                end
            end
            for i=1:vertices
                for j=1:vertices
                    e = zeros(1,vertices);
                    e(i) = e(i) + 1;
                    e(j) = e(j) + 1;
                    if size(find(k-kprime-e>=0),2) == vertices
                        beta = factorial(deg-1)/Pi(k-kprime-e);
                        B = Bi(:,(i-1)*inputs+1:i*inputs);
                        Term = Term + Cfr*beta*[B*Z{j}+Z{j}'*B' W;W zeros(order)];
                    end
                end
            end
        end
    end
    LMIs = LMIs + set(Term < 0);    
end

% determine the number of LMI rows
output.L = 0;
for i=1:size(LMIs,1)
    output.L = output.L + size(LMIs{i},1);
end
% determine the number of scalar variables
output.V = size(getvariables(LMIs),2);

% evaluate the elapsed time to mount the LMIs set
output.cpusec_m = etime(clock,output.cpusec_m);

% solve the LMIs
sol = solvesdp(LMIs,[],sdpsettings('verbose',0,'solver','sedumi'));

% evaluate the elapsed time to solve the LMIs set
output.cpusec = sol.solvertime;

% retrieving the minimal primal residual
p=min(checkset(LMIs));
output.delta = p;

output.feas = 0;
%capturing the solutions (if ones exist)
if p > 0
    output.W = double(W);
    output.Ki = [];
    for i=1:vertices
        K = double(Z{i})*inv(output.W);
        output.Ki = [output.Ki K];
    end
    output.feas = 1;
end

%___________________________________________________________________________________________________
function R = Pi(monomial)

R = 1;
for i=1:size(monomial,2)
    R = R*factorial(monomial(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 [string] = string_monomial(monomial)

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

%___________________________________________________________________________________________________
function max_coef = max_coefficient(k,size_Kfr_d,Kfr_d,d,vertices)

max_coef = -1;
for m=1:size_Kfr_d
    kprime = Kfr_d(m,:);
    if size(find(k-kprime>=0),2) == vertices
        Cfr  = factorial(d)/Pi(kprime);
        if Cfr > max_coef
            max_coef = Cfr;
        end
    end
end
