function Info = hinf_OP08b_OCAM_T3_d_yal(Ai,Bi,Ci,Di,deg,d,param)
%function Info = hinf_OP08b_OCAM_T3_d_yal(Ai,Bi,Ci,Di,deg,d,param)
% evaluate a H-infinity guaranteed cost for a time-invariant discrete-time polytopic 
% system via a homogeneous polynomially parameter-dependent Lyapunov function of arbitray 
% degree using Polya's relaxations. The extended bounded real lemma (with slack variables)
% is used. The condition is given in Theorem 3 of (Oliveira & Peres, OCAM, 2008). The LMIs 
% are programmed using YALMIP and can be solved by any LMI solver  supported by YALMIP 
% (SeDuMi is the default).
%
% inputs: Ai,Bi,Ci,Di   -> vertices of the polytope (Ai = [A1 A2 ... AN], Bi = [B1 .. BN],...)
%         deg           -> degree of the polynomially parameter dependent Lyapunov function
%         d             -> number of Pólya's relaxations
%         param         -> optional parameters
%         param.with_P_def_pos  -> add the constraint P_g(alpha) > 0. If at least one point inside
%                                 the polytope (for instance, one of the vertices) is known 
%                                 to be stable, this constraint is not required.
%        param.hinf             -> if this parameter is different of zero, the routine will
%                                 check the feasibility of the LMIs for this value of the hinf
%                                 norm.
% outputs: Info.hinf     -> H-infinity 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.Pk       -> Solution variables Pk
%          Info.K        -> number of scalar variables used in the optmization problem
%          Info.L        -> number of LMI rows used in the optmization problem
%          Info.delta    -> minimal primal residual returned by the LMI solver (SeDuMi is the default).
%
%example: 2 states and 2 vertices
% A1 = [-0.1  1.1;   
%       -0.8  -0.3];  
% A2 = [-0.6  0.6;
%       0.4   -0.3];
% Ai = [A1 A2];
% Bi = [1 2;0 0]; Ci = [1 0 1 0]; Di = [0 0];
% Info = hinf_OP08b_OCAM_T3_d_yal(Ai,Bi,Ci,Di,1,0)
%
% Date: 23/04/2007
% Author: ricfow@dt.fee.unicamp.br

if nargin == 7
    if isfield(param,'with_P_def_pos')
        with_P_def_pos = param.with_P_def_pos;
    else
        with_P_def_pos = 0;
    end
    if isfield(param,'hinf')
        hinf = param.hinf;
    else
        hinf = 0;
    end
else
    with_P_def_pos = 0;
    hinf = 0;
end

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

Info.cpusec_m = clock;

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

if hinf == 0
    mu = sdpvar(1,1);
    obj = mu;
else
    mu = hinf^2;
    obj = [];
end

%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(['P',mon_string,' = sdpvar(order,order,''symmetric'');']);
    eval(['X',mon_string,' = sdpvar(2*order+inputs+outputs,order+outputs,''full'');']);
end

if deg > 1
    Kfr_g_d = gen_coefs(vertices,deg + d);
    Kfr_d = gen_coefs(vertices,d);
else
    Kfr_g_d = gen_coefs(vertices,deg);
    Kfr_d = gen_coefs(vertices,0);
end
number_LMIs = size(Kfr_g_d,1);
size_Kfr_d = size(Kfr_d,1);

if with_P_def_pos == 1
    % P(alpha) > 0
    for j=1:number_LMIs

        k = Kfr_g_d(j,:);

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

                P = eval(['P',string_monomial(k-kprime)]);%get the variable index
                Term = Term + Cfr*P;
            end
        end
        LMIs = LMIs + set(Term > 0);
        Info.L = Info.L + order;
    end
end
if deg <= 1
    Kfr_d = gen_coefs(vertices,d);
    size_Kfr_d = size(Kfr_d,1);
end

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

% Bounded real lemma (alpha) < 0
for i=1:number_LMIs

    k = Kfr_g_d_1(i,:);

    Term = zeros(2*order+inputs+outputs);
    for j=1:size_Kfr_d
        kprime = Kfr_d(j,:);
        if size(find(k-kprime>=0),2) == vertices
            Cfr  = factorial(d)/Pi(kprime);
            for p=1:vertices
                if k(p) > kprime(p)
                    e = zeros(1,vertices);
                    e(p) = 1;

                    A = Ai(:,(p-1)*order+1:p*order);
                    B = Bi(:,(p-1)*inputs+1:p*inputs);
                    C = Ci(:,(p-1)*order+1:p*order);
                    D = Di(:,(p-1)*inputs+1:p*inputs);

                    Bfr = [eye(order) -A zeros(order,outputs) -B;zeros(outputs,order) -C eye(outputs) -D];
                    bt = factorial(deg)*(k(p)-kprime(p))/Pi(k-kprime);                                        
                    mon_string = string_monomial(k-kprime-e);
                    P = eval(['P',mon_string]);
                    X = eval(['X',mon_string]);
                    Q = blkdiag([P zeros(order);zeros(order) -P],bt*eye(outputs),-bt*eye(inputs)*mu);

                    Term = Term + Cfr*(Q + X*Bfr + Bfr'*X');
                end
            end
        end
    end
    LMIs = LMIs + set(Term < 0);
    Info.L = Info.L + 2*order + inputs + outputs;
end

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

Info.K = size(getvariables(LMIs),2);

sol = solvesdp(LMIs,obj,sdpsettings('verbose',0,'solver','sedumi'));
Info.cpusec = sol.solvertime;
p=min(checkset(LMIs));
Info.delta = p;
if p > 0
    p = 0;
end

Info.hinf = 0;
%capturing the solutions (if ones exist)
if abs(p) < 1e-5 %adopted precision for the minimum primal residual
    Info.Pk   = [];
    for i=1:number_VAR
        mon_string = string_monomial(K_g(i,:));
        Info.Pk = [Info.Pk double(eval(['P',mon_string]))];
    end
    Info.hinf = sqrt(double(mu));
end

%___________________________________________________________________________________________________
function R = Pi(monomial)

R=prod(factorial(monomial));

%___________________________________________________________________________________________________
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