function plotpoly
% Function  	: PlotPoly                          
% Credate   	: 07/20/1999		                  
% CURDATE		: 11/05/2001   
% Description	: This function draws the polytope related with 
%                each  location  reachable  from  the initial 
%                locations, using GLOBAL_TRANSITIONS structure
% Obs.: It only works for dimension 2 or 3.


% record initial time stamp
time = cputime;

% using all data from data structure
global_var;

% clear figure and make initial arrangements to plot
clf;
hold on;

%  Load  GLOBAL_PIHA.InitialLocation to initloc                         %
for k=1:length(GLOBAL_PIHA.InitialConditions)
    initloc = GLOBAL_PIHA.InitialConditions{k}.initialLocation;
end

%  Plot polytopes of initloc using GLOBAL_AUTOMATON    						%

hold on ;
for i=initloc
    for j=1:length(GLOBAL_AUTOMATON{i}.initstate)
        plot(GLOBAL_AUTOMATON{i}.initstate{j}.polytope)
    end
end
END = 0;

actloc   = unique(initloc);
nextloc  = [];
plotloc  = [];
counter  = 1;
oldloc   = [];
omniloc = unique(initloc);

%  Plot 'tree' of polytopes using GLOBAL_AUTOMATON    						%
while  ~END
   for i=1:length(actloc)
     plotloc = [plotloc GLOBAL_TRANSITION{actloc(i)}];   
   end
   plotloc = unique(setdiff(plotloc,omniloc));
   omniloc  = unique([omniloc plotloc]);
   if isempty(plotloc)
      END =1;
   else
  		actloc  = plotloc;
      plotloc= [];
      counter = counter + 1;
      fprintf(1,'Depth #%4.0f \n',counter)
      fprintf('Drawing  polytopes, please wait... ');%
      for i=1:length(actloc)
         var_location 	= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(1);
  		 if ~(strcmp(var_location,'terminal'))& ~(strcmp(var_location,'out_of_bound'))&...
            ~(strcmp(var_location,'time_limit'));
   			var_cell			= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(2);
            var_state		= GLOBAL_XSYS2AUTO_MAP{actloc(i)}(3);
            hold on;
            plot(GLOBAL_AUTOMATON{var_location}.interior_region{var_cell}.state{var_state}.polytope,[1 0 0]);
            for k=1:length(GLOBAL_AUTOMATON{var_location}.interior_region{var_cell}.state{var_state}.mapping)
               % plot(GLOBAL_AUTOMATON{var_location}.interior_region{var_cell}.state{var_state}.mapping{k},[0 0 1]);
            end
            %ause
        elseif (strcmp(var_location,'out_of_bound'))
            fprintf('Location %4.0f is out_of_bound\n',actloc(i));
 				%save locat actloc;
        end
      end
      fprintf('done! \n');
      pause;
  end
end
nextloc = omniloc;
Elapsed_time = cputime - time



% SECOND PART
% This part was used to draw flowpipes. It's not used anymore
if 0
   
% Initializing data from system dynamics.
A_plus =[ -3.3216            -25.736            0.0;...
          25.736              -3.3216           0.0;...
           3.14631            -5.10887    0.0];

A_minus =[-3.3216           -25.736            0.0;...
          25.736             -3.3216           0.0;...
           3.14631            -5.10887    0.0];

B = [263.824;-349.729;29378];
T = 0.002;
N = 4;
Ninterval = 10;
hard_calc= 0; 
   
fprintf('Press any key to draw flowpipe...\n');
pause
fprintf('Drawing  flowpipes, please wait... \n');
for i=1:length(nextloc)
  var_location 	= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(1);
  if ~(strcmp(var_location,'terminal'))
      if ismember(var_location,GLOBAL_XSYS2AUTO_MAP{GLOBAL_PIHA.InitialLocations}(1))
   		X0=GLOBAL_AUTOMATON{i}.initstate{length(GLOBAL_AUTOMATON{i}.initstate)}.polytope;		
      else   
       	var_cell			= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(2);
      	var_state		= GLOBAL_XSYS2AUTO_MAP{nextloc(i)}(3);
     		X0=GLOBAL_AUTOMATON{var_location}.interior_region{var_cell}.state{var_state}.polytope;
      end
      [AM,b] = overall_system_matrix(GLOBAL_PIHA.SCSBlocks, ...
        GLOBAL_PIHA.Locations{var_location}.q);
  		if rank(AM) == size(AM,1)
		  Ainv = inv(AM);
		else
		  Ainv = [];
		end
      inv = location_invariant(var_location);
      [CE,dE,CI,dI] = linearcon_data(inv);
      [Xsim,Tstamp,Telapsed]= compute_flow_sim(AM,Ainv,b,X0,inv,T);
      Xini = X0;
      Vini = vertices(Xini);
    	eAT = expm(AM*Telapsed/Ninterval);
 		displacement = step_response(AM,Ainv,b,Telapsed/Ninterval);
      for k = 1:Ninterval
    		Vk = transform(Vini,eAT,displacement);
    		Xk = transform(Xini,eAT,displacement);
         CH = polyhedron(Vini|Vk);
         plot(CH,[0.5 0.7 0.6]);
         Vini = Vk;
         Xini = Xk;
      end
      if ~hard_calc
      	eAT = expm(AM*Tstamp);
 			displacement = step_response(AM,Ainv,b,Tstamp);
      	Xpoly  = transform(Xsim,eAT,displacement);
         REG2 	 = polyhedron(vertices(Xsim)|vertices(Xpoly));
         CI 	 = get_param(REG2,'CI'); 
         dI 	 = get_param(REG2,'dI'); 
         REG	 = linearcon([],[],CI,dI);
      else
         REG = psim_lin(AM,b,Xsim,Tstamp,1);
      end
     	for j=1:length(REG)
        	plot(inv&REG(j),[.7 .7 .6]);%[i*0.0005 j*0.2 0.8])
      end
	end
   fprintf(1,'Iteration # %4.0f \n',i)
end
fprintf('done! \n');
pause
end

return
% -----------------------------------------------------------------------------

function [Xsim,Tstamp,Telapsed] = compute_flow_sim(A,Ainv,b,X0,inv,T)

% simulate the dynamics  for each vertice, finding the new set of points
% where vector field leaves the invariant. Only for a `linear` dynamics.
%
% Syntax:
%   "[Xsim,Tstamp] = compute_flow_sim(A,Ainv,b,X0,CI,dI,T)"
%
% Description:
%   Simulate each vertice, approximating the time when trajectory crosses
%   any hyperplane. Returns:
%
%   - The new vertices (For the time when first trajectory hits a hyperplane)
%   
%   - Time stamp between the first and the last crossing of any hyperplane
%
%    The inputs to this function are
%
%   * "A": the system matrix
%
%   * "Ainv": the inverse of "A" if it exists, otherwsie it should be
%     "[]"
%
%   * "b": constant input vector for the affine dynamics
%
%   * "X0": a "linearcon" object represeting the initial set
%
%   * inv: invariant (linearcon object)
%
%   * "T": the time step for the trajectory
%
%   The output "Xsim" is a "linearcon" object,(a polytope) representing the
%   transformed segment approximation. "Tstamp" is a real number recording the
%   time stamp for the flowpipe which englobes the intersection region with 
%   the invariant.
%
% Implementation:
%   To approximate the segment, we do the following:
%   
%   1) simulate the points until the first trajectory hits a hyperplane. 
%      Record the value of the vertices at that time and also the time value.
%
%   2) continue simulation until the last trajectory  hits a hyperplane.
%      Record the time value.
%
%    The simulation of each point uses the following expression:
%
%   "x(T) = e^{A*T}*x(0) + inv(A)*(e^{A*T} - I)*b"
%
% See Also:
%   stretch_func_lin,step_response,psim_lin,fs_lin_map,linearcon,transform,
%   clean_up

timing = 0;
init_T = T;
max_counter = 5;
status=[];%approx_param.max_bissection;
%eAT 	 = expm(A*T);
%displacement = step_response(A,Ainv,b,T);

% Transfrom sample points by eAT
crossing = 0;
bissection_counter = 0;

status  = check_crossing(X0,inv);
if strcmp(status,'outside') | strcmp(status,'partially')
   crossing = 1;
   bissection_counter = max_counter;
   fprintf(1,'initial set has 1 or more vertices outside (or in the border) of the invariant\n')
end
while (~crossing) | (bissection_counter <= max_counter)
	eAT 	 = expm(A*T);
	displacement = step_response(A,Ainv,b,T);
   Xt = transform(X0,eAT,displacement);
   % Check if any point cross a hyperplane
   status = check_crossing(Xt,inv);
   if strcmp(status,'inside')
   	X0 = Xt;   
      crossing = 0;
      timing = timing + T;
   else
      crossing = 1;
      T = T/2;
      bissection_counter = bissection_counter + 1;
   end
end
Telapsed = timing;
Xsim = X0;
T = init_T;
all_crossing = 0;
bissection_counter = 0;
time_stamp = 0;
while (~all_crossing) | (bissection_counter <= max_counter)
	eAT 	 = expm(A*T);
	displacement = step_response(A,Ainv,b,T);
   Xt = transform(X0,eAT,displacement);
   % Check if all point cross a hyperplane
   status = check_crossing(Xt,inv);
   if strcmp(status,'outside')
      all_crossing = 1;
      bissection_counter = bissection_counter + 1;
      if (bissection_counter > max_counter)
         time_stamp = time_stamp + T;
      end
      T = T/2;
   else
      X0 = Xt;
      time_stamp = time_stamp + T;
      all_crossing = 0;
   end
end
Tstamp = time_stamp;

return

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

function status = check_crossing(Xt,inv)

% Check whether any point of Vt cross the boundary of the invariant INV.
%
% Xt               : Region to be tested (linearcon object)
% inv              : Invariant.
% 
% Returns:
%
% status				: An string informing if the whole set is "inside","outside" or "partially".
%						  status returns 'error' whether Xt or inv is empty.

if isempty(Xt) | isempty(inv)
   status = 'error';
   fprintf(1,'Check_crossing routine: invariant or region is empty \n')
else
   if ~isfeasible(Xt,inv)
   	status = 'outside';
	elseif isempty(minus(Xt,Xt&inv))
      status = 'inside';
   else     
      status = 'partially';
   end
end     
return