% Input:
% L: matrix (row-wise directions vectors)
% delta: double (timestep)
% U: function handle (support function of the input set)
% X0: function handle (support function of initial set)
% N: integer (time horizon)
% state: cell array of structs (state information)
% options: cell array

% Output:
% results: matrix (row-wise evaluation results for the correspondent
% direction

% Description:
% Computes the evaluations for the different directions in L up to time
% horizon N using the spaceEx Algorithm. The algorithm has been extended
% such that intersections with the states invariant are handled

function [results] = algoInv2(L,delta,U,X0,N,state,options)
starttime = tic;
results = [];

global iteration;

% test intersection of X0 with invariant
values = sptotemplatepolyhedra(X0,L);

% 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);   

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);

    % initial values
    U = @(l) U(state.B'*l);
    [~,phi2,~] = calcphis(abs(state.A),delta);
    [~,~,eA] = calcphis(state.A,delta); 
    [~,~,epsilonpsi,e_p,e_n] = calcepsilon(phi2,state.A,eA,X0,U);

    switch (options{23})
       case 1
          %spaceEx
          Omega0delta = calcomega0delta(eA,delta,U,X0,epsilonpsi,e_p, e_n,0,optimset('Display','off','Algorithm','active-set'));
       case 2
          %PreciseOmega0
          Omega0delta = calcPinitAlternative(state.A,delta,X0,U);
    end;
    %Omega0delta= calcomega0delta(eA,delta,U,X0,epsilonpsi,e_p, e_n,0,optimset('Display','off','Algorithm','active-set'));
    psidelta = calcpsidelta(delta,U,epsilonpsi);
    fprintf('initial values calculated after: %f \n\n',toc(starttime));

    results = NaN(size(L,1),N);
    upperbound = N;

    etA = 1;

    % Intersect Omega0delta with invariant (there is always an intersection
    % cause X0 intersects the invariant) but keep Omega0delta unchanged
    if ~isempty(state.invariant)
        values = sptotemplatepolyhedra(Omega0delta,L);
        values(state.invariant_constraints_in_L) = min(values(state.invariant_constraints_in_L),state.d);
        results(:,1) = sptotemplatepolyhedra(reconstructFunctionHandle(L,values,options),L);
    else
        results(:,1) = sptotemplatepolyhedra(Omega0delta,L);
    end;

    psi = zeros(size(L,1),1);

    v = [zeros(1,size(L,2)-1) 1];
    if ~ismember(v,L,'rows')
       extendedL = [L; v; -v];
    else
       extendedL = L;
    end;
  
    h = waitbar(0,'','Name','Computation in Progress');
    waitbar(0,h,sprintf('Flowpipe Construction progress for iteration %i',iteration)) ;

    intersections = [];
    for i=2:1:N
        % compute next step
        etA = etA * eA;

        % evaluation in all directions (needed to test efficiently for
        % invariant intersections)
        parfor j=1:size(L,1)
            l = L(j,:)';
            psi(j) = psi(j) + psidelta( etA' * l);
            results(j,i) = Omega0delta(etA' * l) + psi(j);
        end;

        % test for inv intersection
        if ~testIntersection(state.d,state.mirrored_invariant_constraints_in_L,results(:,i))
            intersections = [intersections i];
            upperbound = i-1;
            break;  % in case there is no intersection with the invariant abort the computation of further values
        end;

        if ishandle(h)
            waitbar(i/N,h,sprintf('Flowpipe Construction progress for iteration %i',iteration)) ;
        end;

    end;

    % keep only the part of results that contains valid values
    results = results(:,1:upperbound);

    % intersect the results with the invariant
    % results to change using min could be determined considering each
    % invariant constraint as an equality constraint (x2 but only one additional test required) and to compute the
    % min only for those sets which intersect at least on equality constraint
    % => the min does not need to be computed because it is assured that in
    % those directions the invariant value can be used
    if ~isempty(state.C)
        tocut = NaN(1,0);
        for i=1:upperbound
            if testIntersection(-results(state.invariant_constraints_in_L,i),1:length(state.invariant_constraints_in_L),state.d)
                tocut = [tocut i];
                results(state.invariant_constraints_in_L,i) = min(results(state.invariant_constraints_in_L,i),state.d);
            end;
        end;
        tocut = tocut(~isnan(tocut));

        % reconstruct and evaluate the sets which have a part lying inside and
        % outside of the invariant -> this avoids problems with the plot for
        % uncut directions
        for i=1:length(tocut)
            omega = calcPolytopeSP(extendedL,[results(:,tocut(i)); options{32}],options{2},options{3},zeros(size(L,2),1),[],[]);
            results(:,tocut(i)) = sptotemplatepolyhedra(omega,L);
        end;
    end;

    if ishandle(h) 
        close(h);
    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;