% Input:
% L: Matrix (Direction vectors - rowwise)
% delta: double (stepsize)
% U: function handle (Support function of free input)
% X0: function handle (Support function of initial set)
% N: Integer (number of sets to generate)
% state: structure
% options: cell array (parameters)

% Output:
% omegas: cell array of function handles (support functions)
% results: Matrix (results of the evaluation of the support functions
% generated in directions L)

% Description:
% Uses the Algorithm 7.2 from the dissertation from Colas Le Guernic "Calcul
% d'Atteignabilité des Systèmes Hybrides à Partie Continue Linéaire" 2009
% page 96 to generate the evaluations in the different directions. 
% It has been extended to cut the part of the generated sets
% which fall out of the invariant. Furthermore, the while condition was
% extended to consider only the specified time horizon

function [omegas,results] = algoInv(L,delta,U,X0,N,state,options)
global inf;
I = options{22};
if isempty(options{22})
    I = @(l) inf;
end;

omegas = [];
A = state.A;
B = state.B;

% take the evaluated values cause the directions might have been normalized 
% and thus the values from states.d are no longer valid
%state.d = state.complete_invariant_evaluation(state.invariant_constraints_in_L);

values = sptotemplatepolyhedra(X0,L);
if testIntersection(state.d,state.mirrored_invariant_constraints_in_L,values) 
    
    % intesect X0 with invariant
    values(state.invariant_constraints_in_L) = min(values(state.invariant_constraints_in_L),state.d);
    X0 = reconstructFunctionHandle(L,values,options);

    % initialize values for algorithm
    results = zeros(size(L,1),N);
    eA = expm(delta*A); %Phi
    pV = calcpV(B,U);
    [~,phi2,~] = calcphis(abs(A),delta);
    [~,~,epsilonpsi,e_p,e_n] =calcepsilon(phi2,A,eA,X0,pV);

    switch (options{23})
        case 1
            %spaceEx
            X0 = calcomega0delta(eA,delta,pV,X0,epsilonpsi,e_p, e_n,0,optimset('Display','off','Algorithm','active-set'));
        case 2
            %PreciseOmega0
            X0 = calcPinitAlternative(A,delta,X0,pV);
    end;
    psidelta = calcpsidelta(delta,pV,epsilonpsi);
    
    % Intersect Omega0(X0) with invariant (there is always an intersection
    % cause X0 intersects the invariant)
    values = sptotemplatepolyhedra(X0,L);
    values(state.invariant_constraints_in_L) = min(values(state.invariant_constraints_in_L),state.d);
    set1 = reconstructFunctionHandle(L,values,options);
    results(:,1) = sptotemplatepolyhedra(set1,L);  

    
    % actual flowpipe generation
    reducedL = options{33};
    min_it = zeros(size(reducedL,1),1);
    tempres = cell(0);  %temporary
    for k=1:size(reducedL,1)
        if reducedL(k,2) ~= 0
            l2 = L(reducedL(k,2),:)';
        else
            l2 = [];
        end;
        
        [tempres{k},j] = parallelizationHelper(zeros(2,N),N,eA,X0,psidelta,I,L(reducedL(k,1),:)');

        min_it(k) = j;
    end;
    
    for k=1:length(tempres)
        results(reducedL(k,1),:)=tempres{k}(1,:);
        results(reducedL(k,2),:)=tempres{k}(2,:);
    end;

    results = results(:,1:min(min_it));

    %omegas = cell(1);
    additional_dim_option = options{28};
    for i=1:size(results,2)
        if ~isempty(additional_dim_option)
            % consider additional dimension
            omega = reconstructFunctionHandle(L,results(:,i),options);
        else
            omega = calcPolytopeSP(L,results(:,i),options{2},options{3},zeros(size(L,2),1),[],[]);
        end;
        results(:,i) = sptotemplatepolyhedra(omega,L);
    end;
else
    results = [];
end;


% Description:
% Helper method to make AlgoInv executable in parallel
function [res,j] = parallelizationHelper(res,N,eA,X0,epsilonpsi,I,l)
j=1;

r_old = l;
r_i = l;

s_old = 0;
s_old_minus = 0;
s_i = 0;
s_i_minus = 0;

k_old = Inf;
k_old_minus = Inf;
k_i = Inf;
k_i_minus = Inf;

p_omega = X0(l);
p_I = I(l);
p_I_minus = I(-l);
I_minus = -p_I_minus;
I_plus = -p_I;
        
res(1,1) = min(p_omega,p_I);    
res(2,1) = min(X0(-l),p_I_minus);    
while res(1,j) >= I_minus && j < N && res(2,j) >= I_plus
            j = j+1;
                    
            r_i = eA' * r_old;
            
            s_i = s_old + epsilonpsi(r_old);
            s_i_minus = s_old_minus + epsilonpsi(-r_old);  
            
            p_omega = X0(r_i) + s_i;
            p_omega_minus = X0(-r_i) + s_i_minus;
            
            k_i = min(k_old,p_I);
            k_i_minus = min(k_old_minus,p_I_minus);
            
            p_I = I(r_i) + s_i;
            p_I_minus = I(-r_i) + s_i_minus;

            res(1,j) = min(p_omega,k_i);
            res(2,j) = min(p_omega_minus,k_i_minus);

            r_old = r_i;
            s_old = s_i;
            s_old_minus = s_i_minus;
            k_old = k_i;
            k_old_minus = k_i_minus;

            if ((p_omega_minus>I_plus && res(1,j) < I_minus) || (p_omega>I_minus && res(2,j) < I_plus))    % if it does not fall into the invariant, cut it later out and stop analysis
                j = j-1; 
                return;
           end;           
end;

% Input:
% d: vector (invariant values)
% minus_invariants_in_L: vector of integers (index mapping for the opposite
% directions of the invariant directions)
% results: vector (evaluation results for the set to test for invariant
% intersection)

% Output:
% intersection: boolean

% Description:
% fast test for intersection
function [intersection] = testIntersection(d,minus_invariants_in_L,results)
intersection = true;
if ~isempty(minus_invariants_in_L)
    intersection = sum(-results(minus_invariants_in_L) <= d) == size(minus_invariants_in_L,1);    
end;