% Input:
% N: Integer (number of sets which will be in the flowpipe)
% delta: double (sampling time)
% angle: double (angle between successive generated directions in one plane)
% maxRecDepth: Integer (number of locations to consider during analysis)
% scenario: String
% elapsedtime: double (time elapsed before starting the analysis - usually
% 0 unless a stopped analysis will be continued)
% options: cell array containing various options for the analysis
%   {1}: string ('Evaluation' or 'Construction' possible value depend on the chosen scenario)
%   {2}: string (algorithm to use for the optimization method in {3})
%   {3}: string ('linprog'/'fmincon'/'mpt'/'cvx')
%   {4}: integer (1 to enable pre-clustering, 0 else)
%   {5}: ~
%   {6}: string ('fminsearch'/'fminunc'/'dichotomicSearch'/'RayAlgo'/'fast_intersetion')
%   {7}: string ('fminbnd'/'fmincon'/'RayAlgo'/'fast_intersetion')
%   {8}: ~
%   {9}: integer (1 if options shall be imported from .m file)
%   {10}: struct (with string fields path and name and cell type with string '.m' or '.mdl' specifying the input file)
%   {11}: ~
%   {12}: string (containing the form of input U 'box'/'zonotope'/'polytope' if those choices are available)
%   {13}: double (scale factor for 'Scale' scenario)
%   {14}: ~
%   {15}: ~
%   {16}: vector (containing the definition of an additional spontaneous transition
%   {17}: integer (1 if fixpointdetection is enabled, 0 else)
%   {18}: integer (interval for fixpoint checks)
%   {19}: double (tolerance for fixpoint detection)
%   {20}: struct (defining the initial set if provided by the gui - *.mdl)
%   {21}: struct (defining the input set if provided by the gui - *.mdl)
%   {22}: ~ (will be overwritten during invariant handling)
%   {23}: integer (specifying which overapproximation to use with AlgoInv respectively ConstU)
%   {24}: DEPRECATED: integer (1 if the invariants shall be transformed to guards, 0 else)
%   {25}: integer (defining the transition strategy to use -1:Manual choice 2:Most intesections 3: First detected 4: Last detected 5:All
%   {26}: string (optimization method for fmincon to use in calcR)
%   {27}: integer (1 if the last computed flowpipe shall be cut after detecting a transition, 0 else)
%   {28}: ~ (will be overwritten with b of the current state)
%   {29}: ~ (will be overwritten with [C d] of the current states invariant)
%   {30}: integer (>0 if interval output shall be produced - may introduce
%         additional directions, 0 else)
%   {31}: ~ (will be overwritten with a boolean indicating if the
%            invariant handling is enabled)
%   {32}: ~ (will be overwritten with tolerance along additional dimension)
%   {33}: ~ (will be overwritten with a mapping containing the "core"
%            directions of L)
% continue_computation: integer (if >0 the call should continue a previously stopped
% and stored computation state with id continue_computation)

% Output:
% results: Matrix (containing the support function evaluations for the
% generated directions)
% L: Matrix (containing row-wise direction vectors)
% intersectionindices: vector (containing the indices of sets which were
% reachable and intersected a guard before taking a transition)
% omegas: cell array of function handles (containing the support functions
% of the flowpipe sets)
% fixpointindices: vector containing the indices of the sets for which a
% fixpoint has been detected
% interval_information: structure containing information to generate a
% interval representation

% Decription:
% Performs the reachability analysis (might continue a previously analysis
% whose computation state has been stored.

function [results,L,intersectionindices,omegas,fixpointindices,interval_information,totaltime] = hybridReachabilityAnalysis(N,delta,angle,maxRecDepth,scenario,elapsedtime,options,continue_computation)

N = uint32(N);

compeletetime = tic;
global iteration;
global current_path;
global trace;

iteration = 0;
current_path = [];  % initially there is no analysis trace


global precision;
precision = 10000000;

global inf;

if (strcmp(options{3},'mpt') && strcmp(options{2},'GLPK'))
    %inf = Inf;
    inf = 100000;
else
    inf = 100000;
    %inf = Inf;
end;
%inf = realmax;
%inf = 100000;
%inf = 10000000000;
%inf = Inf;

fprintf('Read input \n');
% load struct with input parameters
input = options{10};
if isempty(input)
    error('No valid input file specified!');
else
    % make path available for matlab
    addpath(input.path);
end;

% load information from model file
%[~,~,filetype,n,q,U,qq,dqq,eqq,diqq,eiqq,Rqq,Wqq,qa,I,~,deltafile,Nfile,~,tqq,~,~,transitionqq,transitioniqq,states] = choseFile(input.path,input.name);
[~,~,filetype,~,n,~,states,q,qa,transitions,qq,I,U,deltafile,Nfile] = choseFile(input.path,input.name);

% analyse if the scenario to use supports invariant handling
options{31} = strcmp(scenario,'AlgoInv') || strcmp(scenario,'AlgoInv2') ;

% make option RayAlgo unambiguous to differentiate in intersectflowpipe
% between equality and inequality intersections
flat_option7 = regexp(options{7},'\w*' ,'match');
options{7} = flat_option7{1};
if strcmp(options{7},'RayAlgo')
    options{7} = 'RayAlgo_inequality';
end;
if strcmp(options{6},'RayAlgo')
    options{6} = 'RayAlgo_equality';
end;

    % initializations for a simulink model
    if strcmp( filetype,'.mdl')

        if isempty(options{20}) 
            error('Initial set missing');
        else
            options{11} = 'polyheder';
            I = options{20};
        end;

        if ~isempty(options{21})
            U = options{21};
            options{12} = 'polyheder';
            
        else
           % there is no U (set it to "empty")
           for i=1:length(states)
                states{i}.B = zeros(n,1);
           end;
           options{12} = 'box';
           U.box = 0;
        end;
        
    else
       
        % create missing fields for states structure
        for i=1:1:length(states)
            if ~isfield(states{i},'C')
                states{i}.C = [];
            end;
            
            if ~isfield(states{i},'d')
                states{i}.d = [];
            end;
            
            if ~isfield(states{i},'b')
                states{i}.b = [];
            end;
            
            if ~isfield(states{i},'B')
                states{i}.B = [];
            end;
            
            if ~isfield(states{i},'A')
                states{i}.A = zeros(n);
            end;
        end;
    end;

    % fetch options from m file
    if options{9}
        N = Nfile;
        delta = deltafile;
    end;

    % chose geometric U representation
    if strcmp(options{12},'box')
        U = calcBoxSP(U.box);
    elseif strcmp(options{12},'zonotope') % TODO: up to now, only centered !
        U = calcZonotopeSP(U.zonotope.g);
    elseif strcmp(options{12},'polyheder')
        U = calcPolytopeSP(U.polyheder.C,U.polyheder.d,options{2},options{3},zeros(size(U.polyheder.C,2),1),[],[]);
    elseif strcmp(options{12},'')
        U = @(l) 0;
    else
        error('The chosen representation of the input set is yet not supported.');
    end;
    
    options{15} = angle;
 
fprintf('Generate direction vectors\n');
L = vectorgeneratorEXT(n,angle);

add_dimension = false;
options{32} = [];
for i=1:1:length(states)
    if ~isempty(states{i}.b)
        add_dimension = true;
        
        % here could a tolerance be added to compensate numerical errors
        options{32} = [1;-1]; % e.g. options{32} = [1+3*eps;-1+3*eps];
        break;
    end;
end;

% create missing fields for transitions
for i=1:1:length(transitions)
            if ~isfield(transitions{i},'ResetMatrix')
                transitions{i}.ResetMatrix = eye(n);
            end;
            
            if ~isfield(transitions{i},'spontaneous')
                transitions{i}.spontaneous = false;
            end;
            
            if ~isfield(transitions{i},'eguards_dir')
                transitions{i}.eguards_dir = [];
            end;
            
            if ~isfield(transitions{i},'iguards_dir')
                transitions{i}.iguards_dir = [];
            end;
            
            if ~isfield(transitions{i},'eguards_val')
                transitions{i}.eguards_val = [];
            end;
            
            if ~isfield(transitions{i},'iguards_val')
                transitions{i}.iguards_val = [];
            end;
            
            if ~isfield(transitions{i},'from')
                error('There is a transition without a start location');
            end;
            
            if ~isfield(transitions{i},'to')
                error('There is a transition without a target location');
            end;
            
            if ~isfield(transitions{i},'W')
                transitions{i}.W = [];
            end;
end;

% eliminate redundant guards to speed up intersection computations
%[dqq,eqq,transitionqq,diqq,eiqq,transitioniqq,tqq] = cleanGuards(dqq,eqq,transitionqq,diqq,eiqq,transitioniqq,tqq);
transitions = cleanGuards(transitions);

% if there is a flow with a constant, an additional dimension needs to be introduced to
% deal with constants in the flow equation
if add_dimension
    
    for i=1:length(transitions)
        % adapt guards to deal with additional dimension
        if ~isempty(transitions{i}.eguards_dir)
            transitions{i}.eguards_dir = [transitions{i}.eguards_dir zeros(size(transitions{i}.eguards_dir,1),1)];
        end;

        if ~isempty(transitions{i}.iguards_dir)
            transitions{i}.iguards_dir = [transitions{i}.iguards_dir zeros(size(transitions{i}.iguards_dir,1),1)];
        end;
        
        if ~isempty(transitions{i}.ResetMatrix)
            transitions{i}.ResetMatrix = [transitions{i}.ResetMatrix zeros(n,1); [zeros(1,n) 1]];
        else
            transitions{i}.ResetMatrix = eye(n+1);
        end;
        
        if ~isempty(transitions{i}.W)
            transitions{i}.W = [transitions{i}.W; 0];
        end;
    end;
    
    % adapt direction vectors
    L = [L, zeros(size(L,1),1)];
    
    % adapt Aq, Bq and invariants to deal with additional dimension
    % use Aq and Bq to retrieve information to be compatible with existent
    % m files  -> TODO: should be changed to use only the states array ?
    orig_n = n; % old dimensionality
    n = size(states{1}.A,1)+1; % new dimensionality
    for i=1:1:length(states)
        
        states{i}.A=[states{i}.A,states{i}.b;zeros(1,n)];
        states{i}.B=[states{i}.B;zeros(1,size(states{i}.B,2))];
        
        % adapt invariant
        if options{31}
            states{i}.C = [states{i}.C zeros(size(states{i}.C,1),1)];
            d2 = [repmat(inf,size(vectorgenerator(n),1),1);states{i}.d];
            d2((2*n)-1:2*n)=options{32};

            states{i}.invariant = reconstructOmega([vectorgenerator(n); states{i}.C],d2,L,options{2},options{3},states{i}.b,'Generating invariant representation\n'); %function representation of invariant
        else
            states{i}.C = [];
            states{i}.d = [];
            states{i}.invariant = [];
        end;
        
    end;

    % adapt initial set
    if ~isempty(I.polyheder)
        I.polyheder.C = [I.polyheder.C, zeros(size(I.polyheder.C,1),1); zeros(2,size(I.polyheder.C,2)), [1; -1]];
        I.polyheder.d = [I.polyheder.d; options{32}];
    end;
    
else

    for i=1:1:length(states)
        if options{31}
            states{i}.invariant = reconstructOmega(states{i}.C,states{i}.d,L,options{2},options{3},states{i}.b,'Generating invariant representation\n'); %function representation of invariant
        else
            states{i}.C = [];
            states{i}.d = [];
            states{i}.invariant = [];
        end;
    end;
end;

if options{31}
    for i=1:1:length(states)
        if isempty(states{i}.C)
            % in case there is no invariant but invariant handling is
            % enabled
            states{i}.invariant = [];
        end;
    end;
end;

diqq = [];
eiqq = [];
dqq = [];
eqq = [];
tqq = [];
transitioniqq = [];
transitionqq = [];

% Construct initial set
if strcmp(options{11},'polyheder') || isempty(options{11})  
    X0 = calcPolytopeSP(I.polyheder.C,I.polyheder.d,options{2},options{3},zeros(n,1),[],[]);
else
    error('The chosen representation of the initial set is yet not supported.');
end;

    % add spontaneous transitins from GUI
    if ~isempty(options{16})
        for i=1:size(options{16},2)
            addtransnbr = length(transitions)+1;
            transitions{addtransnbr}.from = options{16}(1,i);
            transitions{addtransnbr}.to = options{16}(2,i);
            transitions{addtransnbr}.spontaneous = true;
            transitions{addtransnbr}.timeelapse = options{16}(3,i);
            transitions{addtransnbr}.ResetMatrix = eye(n);
            transitions{addtransnbr}.W = [];
            transitions{addtransnbr}.eguards_dir = [];
            transitions{addtransnbr}.iguards_dir = [];
            transitions{addtransnbr}.eguards_val = [];
            transitions{addtransnbr}.iguards_val = [];
        end;
        
        qq = [qq options{16}([1 2],:)];
    end;

% store transition guard-information in struct and W part (for reset)
transitions_to_delete = []; % store transitions to delete cause of invalid reset-invariant combinations
for i=1:1:length(transitions)   
    % generate function representation of W
    if ~isempty(transitions{i}.W)
        transitions{i}.W_function = calcPolytopeSP([eye(n);-eye(n)],[transitions{i}.W; -transitions{i}.W],options{2},options{3},zeros(n,1),[],[]);
    else
        transitions{i}.W_function = @(l) 0;
    end;
    
    % store the guard as function
    %transitions{i}.guard_function = calcPolytopeSP([transitions{i}.eguards_dir;-transitions{i}.eguards_dir;transitions{i}.iguards_dir],[transitions{i}.eguards_val;-transitions{i}.eguards_val;transitions{i}.iguards_val],options{2},options{3},zeros(n,1),[],[]);
    
    if options{31}
        % generate I_star
        stateindex = find(q == qq(2,i));
        if stateindex > length(states)
            error('there is a transition to a non-existent state. Does the GUI define a spontaneous transition to an invalid state index?');
        end;
        targetI = states{stateindex}.C;
        transitions{i}.I_star_constraints = zeros(size(targetI));
        transitions{i}.I_star_values = zeros(size(targetI,1),1);
        keep = [];
        for j=1:1:size(targetI,1)
            transitions{i}.I_star_constraints(j,:) = targetI(j,:)*transitions{i}.ResetMatrix;
            transitions{i}.I_star_values(j) = states{stateindex}.d(j) + transitions{i}.W_function(-targetI(j,:)');

            temp = round(transitions{i}.I_star_constraints(j,:)*10000)/10000; % add zero tolerance for direction
            if sum(temp == 0) ~= length(temp)   % temp is 0 vector
                keep = [keep j];
            else
                % ignore tautologies
                % mark transition to delete afterwards in case constraint
                % is unsatisfiable
                if transitions{i}.I_star_values(j)< 0
                    transitions_to_delete = [transitions_to_delete i];
                end;
            end;
            
        end;
        transitions{i}.I_star_constraints = transitions{i}.I_star_constraints(keep,:);
        transitions{i}.I_star_values = transitions{i}.I_star_values(keep);
    else
        %transitions{i}.Istar = [];
        transitions{i}.I_star_constraints = [];
        transitions{i}.I_star_values = [];
    end;
    
end;

% delete transitions with invalid invariant-reset combinations
t_temp = cell(0);
qq_temp = [];
for i=1:length(transitions)
    if ~ismember(i,transitions_to_delete)
        t_temp{length(t_temp)+1} = transitions{i};
        qq_temp = [qq_temp qq(:,i)];
    end;
end;
transitions = t_temp;
qq = qq_temp;

% define structure with information for interval output
interval_information.directions = [];
interval_information.directions_in_L = [];
interval_information.values = [];
if options{30}
    interval_information.directions = vectorgenerator(n);
    if add_dimension
        interval_information.directions = interval_information.directions(1:2*(n-1),:);
    end;
end;

% add to L the directions of guards and invariants
[L,states,transitions,interval_information.directions_in_L] = extendDirections(L,states,transitions,interval_information.directions,q);

% evaluate all invariants (avoids multiple evaluations of same invariants
% when resets are used
for i=1:length(states)
    if options{31} && ~isempty(states{i}.invariant)
        states{i}.complete_invariant_evaluation = sptotemplatepolyhedra(states{i}.invariant,L);
    else
        states{i}.complete_invariant_evaluation = [];
    end;
end;

% construct final diqq, eiqq and transitioniqq considering all invariants
% for all transitions -> corresponds to G* from "Flowpipe-Guard Intersection for Reachability Computations with Support Functions"
% add only those constraints which define a new or stricter constraint
% (delete corse constraints if already included)
for i=1:1:length(transitions)
    
    tempdiqq = [];
    tempeiqq = [];
    
    if ~isempty(transitions{i}.iguards_dir)
        tempdiqq = [transitions{i}.iguards_dir'];
        tempeiqq = [transitions{i}.iguards_val'];
    end;
    
    if ~isempty(transitions{i}.I_star_constraints)
        tempdiqq = [tempdiqq transitions{i}.I_star_constraints'];
        tempeiqq = [tempeiqq transitions{i}.I_star_values'];
    end;
    
    tempdiqq = [tempdiqq; i*ones(1,size(tempdiqq,2))];
    
    
    %tempeiqq = [transitions{i}.iguards_val' transitions{i}.I_star_values'];
    
    tempdiqq = tempdiqq';
    [reducedtemp,~,indexes] = unique(tempdiqq,'rows','stable');
    
    transitions{i}.eiqq = zeros(1,size(reducedtemp,1));
    for j=1:1:max(indexes)
        x = indexes == j;
        [transitions{i}.eiqq(j),b] = min(tempeiqq(x));
        indexes(x) = 0;
        
        tsum = 0;
        for k=1:length(x)
            if x(k) == 1
                tsum = tsum +1;
                if b == tsum
                    indexes(k) = j;
                end;
            end;
        end;
        
    end;
    
    transitions{i}.iguard_constraints_in_diqq = indexes(1:size(transitions{i}.iguards_dir,1));
    transitions{i}.istar_constraints_in_diqq = indexes(size(transitions{i}.iguards_dir,1)+1:end);
    
    reducedtemp = reducedtemp';
    
    transitions{i}.diqq = reducedtemp(1:size(reducedtemp,1)-1,:);
    if ~isempty(transitions{i}.diqq)
        diqq = [diqq transitions{i}.diqq];  
        eiqq = [eiqq transitions{i}.eiqq];
        transitioniqq = [transitioniqq reducedtemp(size(reducedtemp,1),:)];
    end;
    
    if ~isempty(transitions{i}.eguards_dir)
        dqq = [dqq transitions{i}.eguards_dir'];
        eqq = [eqq  transitions{i}.eguards_val']; 
        tqq = [tqq NaN(1,length(transitions{i}.eguards_val))];     
    end;
    transitions{i}.dqq = [transitions{i}.eguards_dir'];
    transitions{i}.eqq = [transitions{i}.eguards_val'];
    transitionqq = [transitionqq i*ones(1,length(transitions{i}.eguards_val))];
    
    if  transitions{i}.spontaneous
            %spontaneous transition
            dqq = [dqq NaN(n,1)];
            eqq = [eqq NaN];
            tqq = [tqq transitions{i}.timeelapse];
            transitionqq = [transitionqq i];
    end;
    
    % store mapping between -diqq, diqq and L
    transitions{i}.diqq_in_L = computeMappingToL(transitions{i}.diqq',L);
    transitions{i}.minusdiqq_in_L = computeMappingToL(-transitions{i}.diqq',L);
    
    temp = inf*ones(size(L,1),1);
    temp2 = temp;
    temp(transitions{i}.diqq_in_L) = transitions{i}.eiqq;
    temp2(transitions{i}.eguard_constraints_in_L(1:length(transitions{i}.eguard_constraints_in_L)/2)) = transitions{i}.eqq;
    temp2(transitions{i}.eguard_constraints_in_L(((length(transitions{i}.eguard_constraints_in_L)/2)+1):end)) = -transitions{i}.eqq;
    transitions{i}.sortedValues = min(temp,temp2);
end;

for i=1:length(transitions)
    temp = inf*ones(size(L,1),1);
    
    for j=1:length(transitions{i}.eguard_constraints_in_L)
        if j <= length(transitions{i}.eqq)
            temp(transitions{i}.eguard_constraints_in_L(j)+length(transitions{i}.eqq)-1) = min(temp(transitions{i}.eguard_constraints_in_L(j)+length(transitions{i}.eqq)-1), -transitions{i}.eqq(j));
        else
            temp(transitions{i}.eguard_constraints_in_L(j)-length(transitions{i}.eqq)+1) = min(temp(transitions{i}.eguard_constraints_in_L(j)-length(transitions{i}.eqq)+1),transitions{i}.eqq(j-length(transitions{i}.eqq)));
        end;
    end;
    
    for j=1:length(transitions{i}.iguard_constraints_in_L)
        temp(transitions{i}.mirrored_iguard_constraints_in_L(j)) = min(temp(transitions{i}.mirrored_iguard_constraints_in_L(j)),transitions{i}.eiqq(j));
        %temp(transitions{i}.iguard_constraints_in_L(j)) = min(temp(transitions{i}.iguard_constraints_in_L(j)),transitions{i}.eiqq(j));
    end;
    
    for j=1:length(transitions{i}.istar_constraints_in_L)
        if transitions{i}.istar_constraints_in_L ~= 0
            %temp(transitions{i}.mirrored_istar_constraints_in_L(j)) = min(temp(transitions{i}.mirrored_istar_constraints_in_L(j)),transitions{i}.eiqq(j));
            %temp(transitions{i}.istar_constraints_in_L(j)) = min(temp(transitions{i}.istar_constraints_in_L(j)),transitions{i}.I_star_values(j));
            temp(transitions{i}.mirrored_istar_constraints_in_L(j)) = min(temp(transitions{i}.mirrored_istar_constraints_in_L(j)),transitions{i}.I_star_values(j));
        end;
    end;
    
    temp = reconstructFunctionHandle(L,temp,options);
    transitions{i}.G_star = zeros(size(L,1),1);
    e = zeros(size(L,1),1);
    t = e;
    parfor j=1:size(L,1)
        [t(j),e(j)] = temp(L(j,:)');
        %t(j) = temp(L(j,:)');
    end;
    transitions{i}.G_star = t;
    
    if ismember(-1,e)
        msgbox(sprintf('Transition %d defines a set of conditions G* build from the invariant and guard conditions which is unsatisfiable due to the formulated constraints or numerical issues.',i),'Unsatisfiable combination of guard/invariant combination found!');
        error('Transition %d defines a set of conditions G* build from the invariant and guard conditions which is unsatisfiable due to the formulated constraints or numerical issues.',i);
    end;
end;

options{33} = reduceL(L);

fprintf('Start processing ...\n');
[results,omegas,intersectionindices,~,fixpointindices] = recursion(q,dqq,eqq,diqq,eiqq,qq,qa,X0,U,delta,N,L,options,maxRecDepth,scenario,tqq,elapsedtime,transitionqq,transitioniqq,states,transitions,continue_computation);

trace{numel(trace) +1} = current_path;

if options{30} 
    if ~isempty(results)
        % generate convex hull over all results along the axes
        interval_information.values = max(results(interval_information.directions_in_L,:),[],2);
    else
        interval_information.values = -inf*ones(size(interval_information.directions_in_L,1),1);
    end;
end;

fprintf('----------------------------------------\n');
totaltime = toc(compeletetime);
fprintf('Complete Analysis terminated after %f\n\n',totaltime);

% copy results as backup to workspace
backup.ResultMatrix = results;
backup.IntersectionIndices = intersectionindices;
backup.FixpointIndices = fixpointindices;
backup.L = L;

assignin('base', 'result_backup', backup);



function [results,omegas,Ijump,qd,fixpointindices] = recursion(q,dqq,eqq,diqq,eiqq,qq,qa,X0,U,t,N,L,options,maxRecDepth,scenario,tqq,elapsedtime,transitionqq,transitioniqq,states,transitions,continue_computation)

global iteration;
global current_path;
if isempty(N)
    error('No valid number of sets in the flowpipe');
end;

% create a trace of the modes being analyzed on the current execution path
% of the hybrid automata
if isempty(current_path)
    current_path.states = qa;
    current_path.transitions = [];
else 
    current_path.states = [current_path.states qa];
end;

results = [];
omegas = [];
qd = [];
Ijump = [];
fixpointindices = [];

results = [];
omegas = [];
qd = [];
Ijump = [];
fixpointindices = [];

% are there further iterations to process
if maxRecDepth > 0
     
    iteration = iteration +1;

    % fetch relevant parameters
    if isnan(qa) || qa < 1
        error('No valid initial state specified!');
    end;
    qa_index = q==qa;
    
    A = states{qa_index}.A;
    b = states{qa_index}.b;
    
    n = size(A,1);
    
    % set options for further processing
    options{22} = states{qa_index}.invariant;
    options{29} = [states{qa_index}.C((2*n)+1:end,:) states{qa_index}.d((2*n)+1:end)]; % options{29} contains the original part of the invariant (non-closed)
    options{28} = b;
    
    fixpoint = -1;  % no fixpoint yet
    
    % Identify possible transitions for current mode qa
    qd = [];
    if ~isempty(qq)
        qd=find(qq(1,:)==qa);   
    end;
    
    construction =  strcmp(options{1},'Construction');
    
    if  continue_computation < 1 % computation is only necessary if no stored state was loaded
        
        % generate flowpipe for Location/Mode qa
        fprintf('\nConstruct Flowpipe for Mode with id %i:\n',qa);
        [omegas,results] = generateFlowpipe(L,X0,U,t,scenario,N,options,states{qa_index});

        if options{17}  % fixpoint check enabled
            fixpoint = checkFixpoint(results,options{18},options{19});
        end;
    else
        omegas = {X0};
        results = sptotemplatepolyhedra(X0,L);  % will be overwriten if stored state is loaded
    end;

    %-----------------------------------------------------------------------------------------------
    % Transition handling
    skip_transition_procedure = 0;
    
    
    if maxRecDepth > 0

        if continue_computation < 1
        
            fprintf('\nCheck for guard intersections ...\n');
            if isempty(omegas)  % no support function representation available -> reconstruct
                fprintf('Reconstructing Omegas as Polytopes ... ');
                for i=1:1:size(results,2)   
                   omegas{i} = calcPolytopeSP(L,results(:,i),options{2},options{3},zeros(size(L,2),1),[],[]);
                end;
                fprintf('done.\n');
            end;

            fprintf('- check guard intersections\n');
            [Ijump,transitiontotake] = getSetsWithGuardIntersectionExt(qd,omegas,results,transitions);
            
            % check if a spontaneous transition has to be taken earlier
            [transition,setindex,spontaneTransition] = checkForSpontaneousTransitions(Ijump,qq,tqq,t,N,elapsedtime,fixpoint,qa,transitionqq);
            if spontaneTransition
                transitiontotake = transition;
                Ijump = setindex;
            else

                % Pick a transition to follow/strategy
                if sum(transitiontotake) > 0 && maxRecDepth > 0
                    [Ijump,transitiontotake] = call_transitionpicker(transitiontotake,Ijump,results,omegas,elapsedtime,iteration,qd,options{25},continue_computation);
               % else
               %     Ijump = [];
               % transitiontotake = [];
                end;

            end;  
            
        else

            % load stored state
            [success,results,omegas,iteration,time,Ijump,transitiontotake,qd] = load_state(continue_computation);
            if ~success %no stored state found with specified id
                error('No stored state with id %d available!\n',continue_computation);
            end;
            qa = qq(1,qd(1));
            elapsedtime = time;
            spontaneTransition = 0; % if transition can be chosen, assume there is no spontaneous transition possible
            [Ijump,transitiontotake] = call_transitionpicker(transitiontotake,Ijump,results,omegas,elapsedtime,iteration,qd,options{25},continue_computation);
            continue_computation = 0;   %reset such that the computation continues in the next recursion
            maxRecDepth = maxRecDepth - iteration +1;   % adjust actual recusion depth

            if isempty(Ijump)
                results = [];
                omegas = cell(0);
            end;      
        end;
        
         % handle fixpoints (have to be plotted seperately
        if fixpoint > 0
            if isempty(Ijump)
                fixpointindices = fixpoint;
            else
                if fixpoint <= Ijump(end)
                    fixpointindices = fixpoint;
                end;
            end;
        end;
    end;
    
    
    if ~isempty(transitiontotake)
   
        Rqq2 = transitions{transitiontotake}.ResetMatrix;
     
        if ~spontaneTransition
        
            fprintf('%  i intersections found for transition(s) to mode with id %i\n',length(Ijump),qq(2,transitiontotake));
    
             %case 1: use matrix representation
            if ~isempty(results) && ~construction
                IjumpResults = results(:,Ijump);

                % precluster enabled: cluster before computing intersections
                if options{4}

                    fprintf('- precluster ...');
                    [cluster,clusteromega] = clustering(omegas(Ijump), results(:,Ijump));

                    Ijump = Ijump(1);

                    %omegas{Ijump(1)} = reconstructOmega(L,cluster,L,options{2},options{3},bq,'Precluster');     %! nicht nötig
                    %if ~isempty(omegas)
                    %    omegas = [omegas(1:Ijump(1)-1), {calcPolytopeSP(L,cluster,options{2},options{3},zeros(size(L,2),1),[],[])}, omegas(Ijump(1):end)];
                    %end;
                    results= [ results(:,1:Ijump(1)-1), cluster];%, results(:,Ijump(1):end)]; % +1 to avoid shift?
                    omegas = omegas(1:Ijump(1));
                    %omegas{end} = clusteromega;
                    % use the same clustering approach
                    omegas{end} = calcPolytopeSP(L,cluster,options{2},options{3},zeros(size(L,2),1),[],[]);
                    fprintf('done.\n');

                    maintainr = cluster;
                    %maintaino = {clusteromegas};
                else
                    maintainr = results(:,Ijump); 
                    %maintaino = omegas(:,Ijump);
                end;
            else
                
                %case 2: there is only the function handle representation used
                % precluster enabled: cluster before computing intersections
                if options{4}

                    fprintf('- precluster ...');
                    [~,clusteromegas] = clustering(omegas(Ijump), []); 

                    Ijump = Ijump(1);

                    %omegas{Ijump(1)} = cluster; 
                    omegas = [omegas(1:Ijump(1)-1), {clusteromegas}, omegas(Ijump(1):end)];
                    maintaino = {clusteromegas};
                    fprintf('done.\n'); 
                else
                    maintaino = omegas(:,Ijump);
                end;
            end;           
            
            fprintf('- calculate intersection sets ... ');  
    
            intersectedResults = [];
            intersectedOmegas =cell(0);
        
            if strcmp(options{7},'fast_intersection') && ~construction
                % handle equality and inequalityguards at the same time
                intersectedResults = intersectFlowpipeFast(L,Ijump,omegas,options,results,transitions{transitiontotake},options{32});
                
            else
                % Inequality guards
                if ~isempty(diqq) && ~skip_transition_procedure

                    [intersectedResults, intersectedOmegas] = intersectFlowpipe(transitions{transitiontotake}.diqq',transitions{transitiontotake}.eiqq',L,Ijump,omegas,options{7},options,results,0,options{32});

                    if ~isempty(intersectedResults)
                        results(:,Ijump) = intersectedResults;
                    end;   
                    
                    if ~isempty(intersectedOmegas)
                        omegas{Ijump} = intersectedOmegas{1:end};
                    end;
                end;

                % Equality guards
                if ~isempty(dqq) && ~skip_transition_procedure
                    [intersectedResults, intersectedOmegas] = intersectFlowpipe(transitions{transitiontotake}.dqq',transitions{transitiontotake}.eqq',L,Ijump,omegas,options{6},options,results,1,options{32});
                end;
            end;
         
            fprintf('done.\n');

            if maxRecDepth > 1 || options{27}
                    if ~construction 
                        %if ~isempty(Ijump)
                        results = results(:, 1:Ijump(1)-1 ); % cut not used results
                        %end;
                        results = [results, maintainr];
                    else
                        omegas = omegas(1:Ijump(1)-1); % cut not used omegas
                        omegas = [omegas, maintaino];
                    end;
            end;
    
             % cluster
             % Assumption: Ijump describes 1 interval
             if ~isempty(transitions{transitiontotake}.dqq) || ~isempty(transitions{transitiontotake}.diqq)
                [clusterresults,clusteromegas] = clustering(intersectedOmegas, intersectedResults);
             else
                 % no intersection possible
                 clusterresults = results(:,size(results,2));
                 clusteromegas = omegas{1:size(omegas,2)};
             end;     

             if  ~skip_transition_procedure
                nextX0 = calcReset(clusterresults,clusteromegas,L,Rqq2,options,transitions{transitiontotake}.W_function,states{q == transitions{transitiontotake}.to},options{31},options{32});
             else
                 nextX0 = X0;
             end;

       else
             % spontaneous transition:
             if ~isempty(results)
                results = results(:,1:setindex);
             else
                omegas = omegas{1:setindex};
             end;

             if  ~skip_transition_procedure
                nextX0 = calcReset(results(:,end),omegas{end},L,Rqq2,options,transitions{transitiontotake}.W_function,states{q == transitions{transitiontotake}.to},options{31},options{32});
             else
                nextX0 = X0;
             end;
       end;  
       
       % log transition
       current_path.transitions = [current_path.transitions transitiontotake];
            
            % store the elapsed time
            elapsedtime = elapsedtime + Ijump(1)*t;
            
            [nextresults,nextomegas,intersectionindices,qd,nextfixpointindices] = recursion(q,dqq,eqq,diqq,eiqq,qq,transitions{transitiontotake}.to,nextX0,U,t,N,L,options,maxRecDepth-1,scenario,tqq,elapsedtime,transitionqq,transitioniqq,states,transitions,continue_computation);
            if ~isempty(results)
                % matrix representation used
                intersectionindices = intersectionindices + size(results,2);
                nextfixpointindices = nextfixpointindices + size(results,2);
            else
                intersectionindices = intersectionindices + length(omegas);
                nextfixpointindices = nextfixpointindices + length(omegas);
            end;
            Ijump = [reshape(Ijump,1,length(Ijump)), reshape(intersectionindices,1,length(intersectionindices))];
            results = [ results, nextresults];
            fixpointindices = [fixpointindices, nextfixpointindices];
            omegas = [omegas, nextomegas];
    else
        fprintf(' Time horizon reached without guard intersection for mode with id %i.\n',qa);
        
        % TODO: handle backtracking for transition strategy: all
    end;
else
    fprintf('Max. recursion depth reached !\n');
end;
