function Info = stabLPV_OdOP_IETCTA_C2_yal(Ai,kappa,dot_alphas)
%function Info = stabLPV_OdOP_IETCTA_C2_yal(Ai,kappa,dot_alphas)
% test the robust stability of a time-varying polytopic system using the notion of kappa-robust
% stability adapted to the time-varying case. The conditions take into account informations about
% up the (kappa-1)-th time-derivative of the uncertain parameters. The slack variables (multipliers)
% are considered affine on beta_1 (first time-derivative of alpha). This condition is formulated by 
% considering the choice C2 given in the paper (Oliveira, de Oliveira & Peres, IETCTA, submitted)
%
% inputs: Ai   -> vertices of the polytope (Ai = [A1 A2 ... AN])
%         kappa -> value of kappa
%         dot_alphas -> bounds on the time-derivatives (dot_alphas = [min_alpha^(1) max_alpha^(1);
%                      min_alpha^(2) max_alpha^(2);.... .... min_alpha^(kappa-1) max_alpha^(kappa-1)];
%
% outputs: Info.feas     -> 1 if robustly stable, 0 otherwise
%          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 optimization problem
%          Info.L        -> number of LMI rows used in the optmization problem
%
%
% Example: 2 states and 2 vertices
% A1 = [0 1;-2 -1];
% A2 = [0 1;-(2+5) -1];
% Ai = [A1 A2];
% dot_1 = [-1 1; -1 1];
% dot_2 = [-2 2;-2 2];
% dot_alphas = [dot_1;dot_2]
% Info = stabLPV_OdOP_IETCTA_C2_yal(Ai,3,dot_alphas)
%
% Date: 08/05/2007
% Author: ricfow@dt.fee.unicamp.br

%dimensions of the system
[order,vertices] = size(Ai);
vertices = vertices / order;

if size(dot_alphas,1) < vertices*max(kappa-1,1)
    fprintf('Missing information about the bounds of the time-derivatives!\n');
    Info = [];
    return;
end

Info.cpusec_m = clock;
%Initializes the description of a new LMI system.
LMIs = set([]);
Info.L = 0;

%generate the polyedron of time-derivatives
H{1} = gen_vectors_h(vertices,dot_alphas((1-1)*vertices+1:1*vertices,:));
M{1} = size(H{1},2);
if kappa > 2
    for i=3:kappa
        H{i-1} = gen_vectors_h(vertices,dot_alphas((i-2)*vertices+1:(i-1)*vertices,:));
        M{i-1} = size(H{i-1},2);
    end
end
calQ_1 = generate_calQ(max(1,kappa-1),vertices,M);
calQ_2 = generate_calQ(max(2,kappa),vertices,M);

R_k = [zeros(kappa,1) eye(kappa)];
L_k = [eye(kappa) zeros(kappa,1)];
for i=1:vertices
    Pk{i} = sdpvar(kappa*order,kappa*order,'symmetric');    
    Qk{i} = kron(L_k',eye(order))*Pk{i}*kron(R_k,eye(order)) + kron(R_k',eye(order))*Pk{i}*kron(L_k,eye(order));        
end
if kappa > 1
    Y = sdpvar(kappa*order, (kappa-1)*order,'full');
end
for i=1:M{1}
    Z{i} = sdpvar((kappa+1)*order, kappa*order,'full');
end


%Pk(a) > YB_{k-1}(a) + B_{k-1}'(a)Y'
for i=1:size(calQ_1,2)
    
    q = calQ_1(:,i);
    B_kappa_q1 = generate_Bkappa(Ai,order,kappa-1,q',H);    
    Term = Pk{q(1)};
    if kappa > 1
        Term = Term - Y*B_kappa_q1 - B_kappa_q1'*Y';
    end
    LMIs = LMIs + set(Term > 0);
    Info.L = Info.L + kappa*order;
end

%Qk(a) + dot(Q)(a) + Z(b_1)B_k(a) + B_k(a)'Z(b_1)' < 0
for i=1:size(calQ_2,2)
    
    q = calQ_2(:,i);
    dotP_q2 = zeros(kappa*order);
    for v=1:vertices
        dotP_q2 = dotP_q2 + Pk{v}*H{1}(v,q(2));
    end
    Qbar_q2 = [dotP_q2 zeros(kappa*order,order);zeros(order,kappa*order+order)];
    
    B_kappa_q2 = generate_Bkappa(Ai,order,kappa,q',H);
    LMIs = LMIs + set(Qk{q(1)} + Qbar_q2 + Z{q(2)}*B_kappa_q2 + B_kappa_q2'*Z{q(2)}' < 0);
    Info.L = Info.L + (kappa+1)*order;
    
    if kappa >= 2
        for q2_bar = q(2)+1:M{1}
            dotP_q2_bar = zeros(kappa*order);
            for v=1:vertices
                dotP_q2_bar = dotP_q2_bar + Pk{v}*H{1}(v,q2_bar);
            end
            Qbar_q2_bar = [dotP_q2_bar zeros(kappa*order,order);zeros(order,kappa*order+order)];
            q_modif = q;
            q_modif(2) = q2_bar;
            B_kappa_q2_bar = generate_Bkappa(Ai,order,kappa,q_modif',H);
            LMIs = LMIs + set(2*Qk{q(1)} + Qbar_q2 + Qbar_q2_bar + Z{q(2)}*B_kappa_q2_bar + B_kappa_q2_bar'*Z{q(2)}' + Z{q2_bar}*B_kappa_q2 + B_kappa_q2'*Z{q2_bar}' < 0);
            Info.L = Info.L + (kappa+1)*order;
        end
    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.delta = min(p);

Info.feas = 0;
%capturing the solutions (if ones exist)
if sum(p < 0) == 0
    Info.feas = 1;
    Info.Pk = [];    
    for i=1:vertices
        Info.Pk = [Info.Pk double(Pk{i})];        
    end
end

%___________________________________________________________________________________________________
function calQ = generate_calQ(kappa,vertices,M);

R{1} = 1:vertices;
for i=1:kappa-1
    R{i+1} = 1:M{i};
end
calQ = all_possible_comb(kappa,R);

%___________________________________________________________________________________________________
function B_kappa = generate_Bkappa(Ai,order,kappa,vec_j,H)

j1 = vec_j(1);
vertices = size(Ai,2)/order;

if kappa == 0
    B_kappa = zeros(order);    
    return;
end    
B_kappa = [Ai(:,(j1-1)*order+1:j1*order) -eye(order) zeros(order,(kappa-1)*order)];
for i=2:kappa
    C = [(0:i-1)' (i-1.-(0:i-1))'];
    B = [];
    for j=i:-1:2
        varsigma = factorial(i-1)/(factorial(C(j,1))*factorial(C(j,2)));
        
        A = zeros(order);
        for v=1:vertices
            A = A + H{j-1}(v, vec_j(j)) * Ai(:,(v-1)*order+1:v*order);
        end
        B = [B varsigma*A];
    end
    B = [B Ai(:,(j1-1)*order+1:j1*order) -eye(order) zeros(order,(kappa-i)*order)];
    B_kappa = [B_kappa; B];
end


%___________________________________________________________________________________________________
function [H] = gen_vectors_h(N,bounds)
%function [H] = gen_vectors_h(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)

if N > 3
    Hinter1 = solve_plane(N-1,limites(2:end,:),limites(1,1)+plano);
    Hinter2 = solve_plane(N-1,limites(2:end,:),limites(1,2)+plano);
    cima1 = [];
    cima2 = [];
    for i=1:size(Hinter1,2)
        cima1 = [cima1 limites(1,1)];
    end
    for i=1:size(Hinter2,2)
        cima2 = [cima2 limites(1,2)];
    end
    Hinter1 = [cima1;Hinter1];
    Hinter2 = [cima2;Hinter2];
    H = [Hinter1 Hinter2];
else
    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
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

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