function Info = hinf_BMOPB08_T1_yal(Ai,B1i,C1i,C2i,D11i,D21i,deg,param)
% function Info = hinf_BMOPB08_T1_yal(Ai,B1i,C1i,C2i,D11i,D21i,deg,d)
% synthesize a H-infinity homogeneous polynomially parameter-dependent filter  of 
% arbitrary degree assuring quadratic stability for linear continuous-time arbitrarily 
% fast time-varying parameters % lying in a polytopic domain.  The condition is given in 
% Theorem 1 of (Borges et al., SIGPRO, 2008). The LMIs are programmed using YALMIP and 
% can be solved by any LMI solver supported by YALMIP (SeDuMi is the default). 
%
% inputs:  Ai,B1i,C1i,C2i,D11i,D21i   -> vertices of the polytopes (Ai = [A1 A2 ... AN], 
%                                       B1i = [B11 B12 ... B1N],...)
%        deg           -> degree of the polynomially parameter-dependent filter
%        param.hinf    -> if this parameter is different of zero, the routine will
%                         check the feasibility of the LMIs for this value of 
%                         the H-infinity 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.K        -> number of scalar variables
%          Info.L        -> number of LMIs rows
%          Info.Z        -> solution variable Z
%          Info.X        -> solution variable X
%          Info.filter   -> matrices of the filter (Afi,Bfi,Cfi,Dfi)
%          Info.delta    -> minimal primal residual returned by the LMI solver 
%
% Example: n=2, N=2
% Ai =[-0.44 0.86  -0.04 -0.67
%      0.63  -1.71 0.07  -0.16];
% B1i = [0.1    0.2
%       0.2    0.1];
% C1i = [0 3 0 2];
% C2i = [-0.4 0.1 -1.1 1.2
%       -1.7  0.3 1.2  0];
% D11i = [0 0];
% D21i = [0.3 -0.2 -0.6 -0.1;
%       0.2  0.7  2.2  0.1];
% Info = hinf_BMOPB08_T1_yal(Ai,B1i,C1i,C2i,D11i,D21i,1)

% Date: 17/08/2007
% Author: ricfow@dt.fee.unicamp.br

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


%determine the number of vertices of polytope
[order,vertices] = size(Ai);
vertices = vertices / order;
[order,inputs] = size(B1i);
inputs = inputs / vertices;
outputs_q = size(C2i,1); %measured output
outputs_p = size(C1i,1); %estimated output


Info.cpusec_m = clock;

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

Info.L = 0;
%generate the exponents of alpha's (power 'deg') to create the variables P
coefs_VAR = gen_coefs(vertices,deg);
number_VAR = size(coefs_VAR,1);%number of variables

%generate the variables
if hinf == 0
    gamma = sdpvar(1);
    obj = gamma;
else
    gamma = hinf^2;
    obj = [];
end

Z = sdpvar(order,order,'symmetric');
X = sdpvar(order,order,'symmetric');
LMIs = LMIs + set([Z Z;Z' X] > 0);
Info.L = Info.L + 2*order;

for i=1:number_VAR
    mon = string_monomial(coefs_VAR(i,:));
    eval(['F',mon,'=sdpvar(outputs_p,order,''full'');']);
    eval(['L',mon,'=sdpvar(order,outputs_q,''full'');']);
    eval(['G',mon,'=sdpvar(order,order,''full'');']);
    eval(['D',mon,'=sdpvar(outputs_p,outputs_q,''full'');']);
end

Kfr_g_1 = gen_coefs(vertices,deg + 1);
number_LMIs = size(Kfr_g_1,1);%number of LMIs to be generated

%Grammian condition

for j=1:number_LMIs

    k = Kfr_g_1(j,:);

    Term_1 = zeros(2*order+inputs+outputs_p);
    for p=1:vertices
        if k(p) > 0
            e = zeros(1,vertices);
            e(p) = 1;


            A   = Ai(:,order*(p-1)+1:p*order);
            B   = B1i(:,inputs*(p-1)+1:p*inputs);
            C1  = C1i(:,order*(p-1)+1:p*order);
            C2  = C2i(:,order*(p-1)+1:p*order);
            D11 = D11i(:,inputs*(p-1)+1:p*inputs);
            D21 = D21i(:,inputs*(p-1)+1:p*inputs);

            bt = factorial(deg)*k(p)/Pi(k);

            mon_string = string_monomial(k-e);
            F = eval(['F',mon_string]);
            L = eval(['L',mon_string]);
            G = eval(['G',mon_string]);
            D = eval(['D',mon_string]);

            T11 = bt*(A'*Z + Z*A);
            T12 = bt*(Z*A + A'*X) + C2'*L' + G';
            T13 = bt*C1'-F'-C2'*D';
            T14 = bt*Z*B;
            T22 = bt*(A'*X + X*A)+C2'*L' +L*C2;
            T23 = bt*C1'-C2'*D';
            T24 = bt*X*B+L*D21;
            T33 = -bt*eye(outputs_p);
            T34 = bt*D11 - D*D21;
            T44 = -bt*gamma*eye(inputs);
            Term_1 = Term_1 + [T11 T12 T13 T14;T12' T22 T23 T24;T13' T23' T33 T34;T14' T24' T34' T44];
        end
    end
    LMIs = LMIs + set(Term_1 < 0);
    Info.L = Info.L + (2*order + inputs + outputs_p);
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

%capturing the solutions (if ones exist)
if abs(p) < 1e-7 %adopted precision for the minimum primal residual
    Info.hinf = sqrt(double(gamma));
    Info.X = double(X);
    Info.Z = double(Z);
    U = eye(order);
    Info.filter.Afi = [];
    Info.filter.Bfi = [];
    Info.filter.Cfi = [];
    Info.filter.Dfi = [];
    for i=1:number_VAR
        mon = string_monomial(coefs_VAR(i,:));
        F = eval(['F',mon]);
        L = eval(['L',mon]);
        G = eval(['G',mon]);
        D = eval(['D',mon]);
        V = inv(U)'*(eye(order) - Info.X*inv(Info.Z));
        Info.filter.Afi = [Info.filter.Afi inv(U')*double(G)*inv(V*Info.Z)];
        Info.filter.Bfi = [Info.filter.Bfi inv(U')*double(L)];
        Info.filter.Cfi = [Info.filter.Cfi double(F)*inv(V*Info.Z)];
        Info.filter.Dfi = [Info.filter.Dfi double(D)];
    end
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