% Input:
% A: Matrix (defining the continuous dynamics)
% delta: double (sampling time)
% x0: function handle (support function of the initial set)
% u: function handle (support function of free input)

% Output:
% res: function handle (support function)

% Description:
% Generates the initial set for the hybrid reachability analysis from
% dissertation Colas Le Guernic "Calcul d'Atteignabilité des Systèmes Hybrides à Partie Continue Linéaire" 2009 pages 77-78.

function [res] = calcPinitAlternative(A,delta,x0,u)

[phi1,phi2,~] = calcphis(abs(A),delta);
edA = expm(delta*A);

au= @(l) u(A'*l);
au_box = boxoperator(au,size(A,2));
temp = @(l) au_box(phi2'*l);
epsilonu = Intervall_hull(temp,size(A,2));

temp = @(l) x0( (A*(eye(size(A,2),size(A,2))-edA))'*l);
temp2 = boxoperator(temp,size(A,2));
temp = @(l) temp2( phi1'*l);
temp2 = Intervall_hull(temp,size(A,2));

temp = @(l) x0((A^2)'*edA'*l);
temp3 = boxoperator(temp,size(A,2));
temp = @(l) temp3(phi2'*l);
temp3 = Intervall_hull(temp,size(A,2));

epsilonx0 = @(l) temp2(l) + temp3(l);

start = 0;

res = @(l) result(edA,delta,u,x0,epsilonu,epsilonx0,start,l);
 

function [value] = result(edA,delta,u,x0,epsilonu,epsilonx0,start,l)

w = @(x) -( (1-x)*x0(l) + x*x0(edA'*l)+x*delta*u(l) +(x^2)*epsilonu(l) + x*(1-x)*epsilonx0(l) );

[~,value] = fmincon( w ,start,[],[],[],[],0,1,[], optimset('Display','off','Algorithm','active-set'));
value = -value;