function [SEG,SPf] = seg_approx_SD_ode(sys_eq,ode_param,X0,SP0,t0,tf,Pcon)

% Approximate a single segment of a flow pipe for a `nonlinear` dynamics.
%
% Syntax:
%   "[SEG,SPf] = seg_approx_ode(sys_eq,ode_param,X0,SP0,t0,tf)"
%
% Description:
%   Compute a conservative approximation of a flow pipe segment in the time
%   interval "[t0,tf]" for the `nonlinear` dynamics "dx/dt = f(x)". The
%   inputs to this function are
%
%   * "sys_eq": string containing file name of the derivative function
%     for the ODE (i.e., overall_system_ode in the directory \util)
%
%   * "ode_param": optional parameters for the ODE file
%
%   * "X0": a "linearcon" object representing initial set
%
%   * "SP0": sample points that are simulated to construct the convex hull,
%     given at time t0
%
%   * "t0"/"tf" : start/final time with respect to "X0"
%
%   * Pcon : linearcon object representing the constraint on the parameters.
%
%   The outputs of this function are
%
%   * "SEG": a "linearcon" object (a polytope) representing the flow pipe
%     segment approximation.
%   
%   * "SFf":  sample points the are simulated from "SP0" to the time "tf".
%
% Implementation:
%   To approximate the flow pipe segment, we first simulate the sample
%   points from times "t = t0" to "t = tf" and compute the convex hull of
%   the sample points at time "t = t0" and "t = tf". After the convex hull
%   is obtained, we extract the matrix-vector pair "[CI,dI]" that represents
%   the linear inequality "CI*x <= dI" for the convex hull. The normal
%   vectors for the faces of the conex hull, which are the rows of the
%   matrix "CI", are used to solve the optimization problem.
%
%
%   
%   "max (x0 in X0, t in [0,T]) ci'*x(t,x0)"
%
%
%
%   where "ci" is the "i"-th normal vector and "x(t,x0)" denotes the
%   solution to the nonlinear differential equation. The above optimization
%   problem is solved by calling the MATLAB routine for solving constrained
%   nonlinear optimization problem "constr". The computation of the
%   objective function, which includes numerical intregration of the ODE to
%   find the solution "x(t,x0)" from "x0" at time "t" is done in the
%   function "stretch_func_ode()".
%
%
%
%   After solving the optimization problem, we adjust the inequality
%   constraints for the convex hull to be "CI*x <= dInew" where "dInew(i)"
%   is the solution obtained for the "i"-th optimization problem. This, in
%   effect, expands the convex hull to cover the flow pipe segment. Put the
%   new linear inequality constraints into a "linearcon" object, call the
%   function "clean_up()" to remove redundant constraints, and return the
%   result.
%   
%
% See Also:
%   stretch_func_lin,step_response,psim_lin,fs_lin_map,linearcon,transform,
%   clean_up

% [SEG,SPf] = seg_approx_ode(sys_eq,ode_param,X0,SP0,t0,tf)
% Compute the outer approximation of the flow pipe segment from t0 to tf

global GLOBAL_APPROX_PARAM

% Simulate sample points from t0 to tf
if tf~=t0
   [SPi,Siminfi] = simulate_points(SP0,sys_eq,ode_param,t0,Pcon);
   [SPf,Siminff] = simulate_points(SP0,sys_eq,ode_param,tf,Pcon);
   SPi=vertices(polyhedron(SPi)); % Removes unnecessary vertices
   CH = polyhedron(SPi | SPf);
elseif t0==0
   SPf = SP0;
   SEG=linearcon(polyhedron(SP0));
   return
else
   [SPf,Siminff] = simulate_points(SP0,sys_eq,ode_param,t0,Pcon);
   CH = polyhedron(SPf);
end

SPf=vertices(polyhedron(SPf)); % Removes unnecessary vertices

% Compute convex hull from these extreme points
[CE,dE,CI,dI] = linearcon_data(linearcon(CH));
[CInew,dInew] = shwrap_ode(CI,sys_eq,ode_param,X0,t0,tf,Pcon,Siminff);
for k = 1:length(dI)
  if (dInew(k) > dI(k))
    dI(k) = dInew(k);
  else
    if (dInew(k) < dI(k) - GLOBAL_APPROX_PARAM.poly_epsilon)
      fprintf(1,['Warning: Optimization result is not reliable, falling' ...
	    ' back to the convex hull approximation\n']);
    end
  end
end
SEG = clean_up(linearcon([],[],CI,dI));
return

% -----------------------------------------------------------------------------

function [Pf, Siminf] = simulate_points(X0,sys_eq,ode_param,tf,Pcon);
X0=vertices(X0);
Siminf=[];
Param=vertices(Pcon);
Pf = vertices;
if isempty(Param) 
    for k = 1:length(X0)
        % syntax: ODE45('F',TSPAN,Y0,OPTIONS,P1,P2,...)
        [T,X] = ode45(sys_eq,[0 tf],X0(k),[],ode_param,[]);
        Pf = Pf | X(end,:)';
        Siminf=[Siminf; struct('X0',X0(k),'P0',[],'T',T,'X',X)];
    end;
else
    for i=1:length(Param)
        for k = 1:length(X0)
            % syntax: ODE45('F',TSPAN,Y0,OPTIONS,P1,P2,...)
            [T,X] = ode45(sys_eq,[0 tf],X0(k),[],ode_param,Param(i));
            Pf = Pf | X(end,:)';
            Siminf=[Siminf; struct('X0',X0(k),'P0',Param(i),'T',T,'X',X)];
        end;
    end;
end;
return

% -----------------------------------------------------------------------------

function [C,d] = shwrap_ode(C,sys_eq,ode_param,X0,t0,tf,Pcon,Siminf)

% Shrink wrap the flow pipe segment in a polytope using the normal vectors
% stored in the rows of the given matrix C, i.e. compute the smallest linear
% constraint set given the set of directions C that contains the flow pipe
% segment from t0 to tf.
% C             : a set of directions stacked together in a matrix
% sys_eq        : a string containing file name of the derivative
%                 function for the ODE
% ode_param     : optional parameters for the ODE file
% X0            : a constraint representing initial set X0
% t0/tf         : start/final time with respect to X0
% Pcon : A linearcon object representing the constraint on the parameters.
%   Siminf : A structure that records information about the trajectory
%       Siminf.X0 = initial point for the trajectory
%       Siminf. P0 = parameter used for computing the trajectory
%       Siminf.T = vector of simulation times for the trajectory
%       Siminf.X = vector of simulated points. 

% Siminfo contains information from the simulation in order to make a better 
% initial guess

% extract linear inequality constraints defining X0

[CE,dE,CI,dI] = linearcon_data(X0);
[CPE,dPE,CPI,dPI] = linearcon_data(Pcon);

% (1) inequality constraints

    [CIn,CIm]=size(CI);
    [CPIn,CPIm]=size(CPI);

    % Remember the dimensions of the state space.
    dimension=CIm;


    if tf~=t0
       fminCI=[CI,zeros(CIn,CPIm+1);zeros(1,CIm) 1 zeros(1,CPIm); zeros(1,CIm) -1 zeros(1,CPIm);zeros(CPIn,CIm+1), CPI];
       fmindI=[dI;tf;-t0;dPI];
    else
       fminCI=[CI,zeros(CIn,CPIm);zeros(CPIn,CIm), CPI];
       fmindI=[dI;dPI];
    end

    % (2) equality constraints (not dependent on t)
    [CEn,CEm]=size(CE);
    [CPEn,CPEm]=size(CPE);

    fminCE=[];
    fmindE=[];
    if ~isempty(CE)
        fminCE=[CE,zeros(CEn,CPIm+1)];%CPIm+1
        fmindE=dE;
    end;
    if ~isempty(CPE)
        fminCE=[fminCE;zeros(CPEn,CIm+1),CPE];%CIm+1
        fmindE=[fmindE;dPE];
    end;

% (3) Set the optimization parameters

V = vertices(X0);
P = vertices(Pcon);
options = optimset;
options.TolFun =0.2*1e-9;%f(3) of foptions
options.iter = 4e8*size(C,2);%f(14) of foptions
options.Display=['off'];
options.Diagnostics=['off'];
options.LargeScale=['off'];

% Stretch along each given direction to cover the flow pipe segment
d = zeros(size(C,1),1);        


% (4) Optimize for each direction

for l = 1:size(C,1)
  n_vector = C(l,:)';
  dlmax = Inf;
  
  if tf~=t0
      % compute the best initial state 
      % Store in " tindex"  the index of the time elements of  the vector Siminf(1).T that are greater than t0
      %
      [ts,tindex]=setdiff(Siminf(1).T.*(Siminf(1).T>=t0),0);
      % Computer the max arg i C'x(i)
      [minnx,nx_index]=min(-n_vector'*Siminf(1).X(tindex,:)');
      Pinit=Siminf(1).P0;
      Xinit=Siminf(1).X0;
      Tinit=Siminf(1).T(tindex(nx_index));
      for k0=2:length(Siminf)
          [ts,tindex]=setdiff(Siminf(k0).T.*(Siminf(k0).T>=t0),0);
          [nx,nx_index]=min(-n_vector'*Siminf(k0).X(tindex,:)');
          if nx<minnx
              minnx=nx;
              Pinit=Siminf(k0).P0;
              Xinit=Siminf(k0).X0;
              Tinit=Siminf(k0).T(tindex(nx_index));
          end;
      end;

    else % tf==t0
      Pinit=Siminf(1).P0;
      Xinit=Siminf(1).X0;
      Tinit=[];

    end;
        
    [Xopt,dl] = fmincon('stretch_func_ode',[Xinit;Tinit;Pinit], fminCI,fmindI, fminCE,fmindE,[],[],[],options,...
             sys_eq,ode_param,n_vector,t0,tf,dimension);
    
    % The end of my hack  <- Ansgar
    if (dl < dlmax)
        dlmax = dl;
    end
    d(l) = -dlmax;
end
return