% -----------------------------------------------------------------------------
% RefinedContour.m
% -----------------------------------------------------------------------------
%
%  This script identifies the region where the supplied function takes 
% the value 1. The function valfun must take the current point as a two
% vector with each element in the range [0,1] and return either 0 or 1.
%
% The algorithm starts by first evaluating the function on a regular grid
% on [0,1]x[0,1] with the number of data points in each indicated by the
% elements of the two vector initial steps. Then, it recursively refines 
% each of the squares in the grid into 2x2 cells until DEPTH is reached.
%
% Finally, it returns an structure which includes the results. points.x
% and points.y include the x and y coordinates of all the evaluated data 
% points, while points.value includes their evaluated function values.
%
%
% -----------------------------------------------------------------------------
%
%   function [ points, griddata ] = ...
%	    RefinedContour( valfun, initialsteps, depth, plotflag );
%
% -----------------------------------------------------------------------------
%  Input Arguments:
%    
%    valfun       : val = valfun( point ) is a function that takes a 2-vector
%                   and returns the corecponding value of the function whose
%                   stability will be assessed.
%    initialsteps : a 2 vector specifying the size of the grid for the first
%                   iteration of the evaluation in the x and y directions. For
%                   example, [6 6] will cause a 6x6 grid of points to be 
%                   evaluated first. 
%    depth    : Maximum depth that the refinement will proceed. Depth of 2 only
%               performs one refinement on the initial grid.
%    plotflag : indicates whether progress plots are enabled or not.
%
% -----------------------------------------------------------------------------
%  Outputs :
%
%     points  : A structure of arrays with three fields.
%
%          points.x     : List of speed values
%          points.y     : List of height values
%          points.value : List of flags indicating stability.
%
%     griddata : A structure which presents the results in a grid form. 
%                   The following fields are generated:
%
%          griddata.curDepth : The final depth of refinement
%          griddata.xcount   : Number of speed grid data points
%          griddata.ycount   : Number of height grid data points.
%          griddata.xpoints  : Values on the speed axis
%          griddata.ypoints  : Values on the height axis
%          griddata.data     : Matrix of stability values
%          griddata.validity : Matrix of flags indicating which runs are actual
%                              simulations, which ones are assumed.
%
% -----------------------------------------------------------------------------
% $Id: RefinedContour.m,v 1.1.1.1 2003/10/28 20:05:59 ulucs Exp $
% -----------------------------------------------------------------------------

function [points,griddata] = ...
	RefinedContour( valfun, initialsteps, depth, plotflag );

% Check inputs
if ( nargin < 4 )
  plotflag = 0;
  if ( nargin < 3 )
    error('Not enough input arguments!');
  end
end

if ( ~isa( valfun, 'char' ) )
  error('Invalid function name!');
end

if ( ~isnumeric( initialsteps ) | (length( initialsteps ) ~= 2) )  
  error('Invalid initial steps specification!');
end

if ( ~isnumeric( depth ) | (length( depth ) ~= 1) )  
  error('Invalid depth specification!');
end

disp( '============================================================' );
disp( '= RefineCountour ===========================================' );
disp( '' );
disp( sprintf('Scanning function %s up to depth %i', valfun, depth ) );


if ( plotflag == 1 ) 
  figure(1);
  clf;
  hold on;
  axis( [0 1 0 1] );
  xlabel( 'x (normalized)' );
  ylabel( 'y (normalized)' );
  title( 'Function value. x:0, o:1' );
end

% Initialize the cell array to store all the results
AllResults = [];
LinearResults = [];

% Setup the initial ranges
xrange = [ 0 1 initialsteps(1) ];
yrange = [ 0 1 initialsteps(2) ];

disp( 'Computing initial grid...' );
% Run the function for the initial grid
AllResults.curDepth = 1;

AllResults.xcount = xrange(3) + 1;
AllResults.ycount = yrange(3) + 1;

AllResults.xpoints = linspace(xrange(1), xrange(2), AllResults.xcount);
AllResults.ypoints = linspace(yrange(1), yrange(2), AllResults.ycount);

[results, LinearResults] = ...
    ScanCell( valfun, AllResults.xpoints, AllResults.ypoints, ...
              LinearResults, 1, plotflag );

AllResults.data = results.values;
AllResults.validity = ones( AllResults.ycount, AllResults.xcount );

disp( 'Starting iterations...' );
% Start recursively refining intervals
AllResults.curDepth = 2;
while ( AllResults.curDepth <= depth )

  disp( sprintf('Computing depth %i...', AllResults.curDepth ) );
  curDepth = AllResults.curDepth;

  % First, extend the data array to accomodate new simulations
  OldResults = AllResults;
  
  AllResults.xcount = (2^(curDepth-1) * xrange(3)) + 1;
  AllResults.ycount = (2^(curDepth-1) * yrange(3)) + 1;
  AllResults.xpoints = ...
	  linspace(xrange(1), xrange(2), AllResults.xcount);
  AllResults.ypoints = ...
	  linspace(yrange(1), yrange(2), AllResults.ycount);
  
  AllResults.data = zeros( AllResults.ycount, AllResults.xcount );
  AllResults.validity = zeros( AllResults.ycount, AllResults.xcount );

  % Copy the contents of the old results into the new result array
  for ( xind = 0:OldResults.xcount-1 )
	for ( yind = 0:OldResults.ycount-1 )
	  AllResults.data( yind * 2 + 1, xind * 2 + 1 ) ...
		  = OldResults.data( yind + 1, xind + 1 );
	  AllResults.validity( yind * 2 + 1, xind * 2 + 1 ) ...
		  = OldResults.validity( yind + 1, xind + 1 );

	end
  end

  % Fill in assumed values for the squares
  for ( xind = 1:OldResults.xcount-1 )
	for ( yind = 1:OldResults.ycount-1 )
	  corners = [ OldResults.data( yind, xind ) 
				  OldResults.data( yind, xind + 1 )
				  OldResults.data( yind + 1, xind )
				  OldResults.data( yind + 1, xind + 1 ) ];

	  corners = (corners == 1);
	  if ( isempty( find( corners ~= corners(1) ) ) )
		AllResults.data( ((yind-1)*2+1):(yind*2+1),...
						 ((xind-1)*2+1):(xind*2+1)) = ...
			corners(1);
	  end
	end
  end
  
  if ( plotflag == 1)
	figure(2);
	clf;
	subplot(2,1,1);
	contour(AllResults.xpoints, AllResults.ypoints, AllResults.validity, 1);
	axis tight;
	subplot(2,1,2);
	[iy, ix] = find(AllResults.data == 1);
	scatter(AllResults.xpoints(ix), AllResults.ypoints(iy),'.');
	hold on;
	[iy, ix] = find(AllResults.data <= 0);
	scatter(AllResults.xpoints(ix), AllResults.ypoints(iy),'r.');
	axis tight;
	figure(1);
  end
  
  % Identify regions in the previous depth which require refining
  prevDepth = curDepth - 1;
  
  % Go through all cells
  for ( xind = 1:OldResults.xcount-1 )
	for ( yind = 1:OldResults.ycount-1 )
	  corners = [ OldResults.data( yind, xind ) 
				  OldResults.data( yind, xind + 1 )
				  OldResults.data( yind + 1, xind )
				  OldResults.data( yind + 1, xind + 1 ) ];

	  corners = (corners == 1);
	  % If there is a change in stability, run another batch of simulations
	  if ( ~isempty( find( corners ~= corners(1) ) ) )

		% Create the ranges for the cell to be refined
		xpoints = linspace( OldResults.xpoints( xind ), ...
                              OldResults.xpoints( xind + 1 ), 3 );
		ypoints = linspace( OldResults.ypoints( yind ), ...
                              OldResults.ypoints( yind + 1 ), 3 );

		% Run the simulations and get the results
		[results, LinearResults] = ...
			ScanCell( valfun, xpoints, ypoints, ...
					  LinearResults, curDepth, plotflag );
          
		% Add the result of the simulation to the appropriate list of the
		% current depth level
		AllResults.data( ((yind-1)*2+1):(yind*2+1),...
						 ((xind-1)*2+1):(xind*2+1)) = ...
			results.values;
		AllResults.validity( ((yind-1)*2+1):(yind*2+1),...
						 ((xind-1)*2+1):(xind*2+1)) = ...
			ones(3,3);
	  end
	end
  end
  
  if ( plotflag == 1)
	figure(2);
	clf;
	subplot(2,1,1);
	contour(AllResults.xpoints, AllResults.ypoints, AllResults.validity, 1);
	axis tight;
	subplot(2,1,2);
	[iy, ix] = find(AllResults.data == 1);
	scatter(AllResults.xpoints(ix), AllResults.ypoints(iy),'.');
	hold on;
	[iy, ix] = find(AllResults.data <= 0);
	scatter(AllResults.xpoints(ix), AllResults.ypoints(iy),'r.');
	axis tight;
	figure(1);
  end
  
  % Go through all the new cells and identify inconsistencies. Run 
  % simulation if necessary 
  nochange = 0;
  while ( nochange == 0 )
	nochange = 1;
	for ( xind = 1:AllResults.xcount-1 )
	  for ( yind = 1:AllResults.ycount-1 )

		valid = AllResults.validity( yind, xind );
		stable = (AllResults.data( yind, xind ) == 1);
		rightvalid = AllResults.validity( yind, xind+1 );
		rightstable = (AllResults.data( yind, xind+1 ) == 1);
		downvalid = AllResults.validity( yind+1, xind );
		downstable = (AllResults.data( yind+1, xind ) == 1);

		if ( (stable ~= rightstable) & (valid == 0 | rightvalid == 0 ) )
		  % Run the point to the right
		  if ( rightvalid == 0 )
			[results, LinearResults] = ...
				ScanCell( valfun, ...
						  AllResults.xpoints(xind+1), ...
						  AllResults.ypoints(yind), ...
						  LinearResults, curDepth, plotflag );
			% Add the result of the simulation to the appropriate point
			AllResults.data( yind, xind+1 ) = results.values(1,1);
			AllResults.validity( yind, xind+1) = 1;
		  else
			[results, LinearResults] = ...
				ScanCell( valfun, ...
						  AllResults.xpoints(xind), ...
						  AllResults.ypoints(yind), ...
						  LinearResults, curDepth, plotflag );
			% Add the result of the simulation to the appropriate point
			AllResults.data( yind, xind ) = results.values(1,1);
			AllResults.validity( yind, xind) = 1;
		  end

		  nochange = 0;
		end
		
		if ( (stable ~= downstable) & (valid == 0 | downvalid == 0 ) )
		  % Run the point to the bottom
		  if ( downvalid == 0 )
			[results, LinearResults] = ...
				ScanCell( valfun, ...
						  AllResults.xpoints(xind), ...
						  AllResults.ypoints(yind+1), ...
						  LinearResults, curDepth, plotflag );
          
			% Add the result of the simulation to the appropriate point
			AllResults.data( yind+1, xind ) = results.values(1,1);
			AllResults.validity( yind+1, xind) = 1;
		  else
			[results, LinearResults] = ...
				ScanCell( valfun, ...
						  AllResults.xpoints(xind), ...
						  AllResults.ypoints(yind), ...
						  LinearResults, curDepth, plotflag );
			% Add the result of the simulation to the appropriate point
			AllResults.data( yind, xind ) = results.values(1,1);
			AllResults.validity( yind, xind) = 1;
		  end
			
		  nochange = 0;
		end
		
	  end
	end
  end
  
  AllResults.curDepth = OldResults.curDepth + 1;
  
  if ( plotflag == 1 ) 
    figure(1);
    clf;
    hold on;
    axis( [0 1 0 1] );
    xlabel( 'x (normalized)' );
    ylabel( 'y (normalized)' );
    title( 'Function value. x:0, o:1' );
    success = find( LinearResults.value == 1 );
    failure = find( LinearResults.value == 0 );
    scatter( LinearResults.x(success), LinearResults.y(success), '.' );
	drawnow;
  end
  
%  disp('Press a key');
%  pause;
end

% Retrieve the results
points = LinearResults;
griddata = AllResults;

% =========================================================================
% Function to evaluate the value function within a given cell. Also 
% eliminates duplicate runs.
% =========================================================================
function [results, LinearResults] = ...
    ScanCell( valfun, xrange, yrange, oldresults, curdepth, plotflag );

results = [];
results.xrange = xrange;
results.yrange = yrange;
results.values = [];

LinearResults = oldresults;

for xind = 1:length(xrange),
  for yind = 1:length(yrange),

    xval = double(single(xrange(xind)));
    yval = double(single(yrange(yind)));
    
    if ( ~isempty( oldresults ) )
      % Find out whether the simulation was run before
      oldx = find( oldresults.x == xval );
      oldy = find( oldresults.y == yval );
      oldcommon = intersect( oldx, oldy );
    else
      oldcommon = [];
    end
    
    if ( isempty( oldcommon ) )
      curval = feval( valfun, [xval, yval] );
      if ( ~isempty( LinearResults ) )
        updateindex = length( LinearResults.x ) + 1;
      else
        updateindex = 1;
      end
      LinearResults.x( updateindex ) = xval;
      LinearResults.y( updateindex ) = yval;
      LinearResults.value( updateindex ) = curval;
        
	  if ( plotflag == 1 ) 
        if ( curval <= 0 )
          plot( xval, yval, 'x' );
        else
          plot( xval, yval, 'o' );
        end
        drawnow;
      end

      if ( curval <= 0 )
        disp(sprintf('Execution %i:[%i/%i, %i/%i]:%f,%f -> unstable(%i)', ...
                     curdepth, ...
                     xind, length(xrange), yind, length(yrange), ...
                     xval, yval, curval ) );
      else
        disp(sprintf('Execution %i:[%i/%i, %i/%i]:%f,%f -> stable', ...
                     curdepth, ...
                     xind, length(xrange), yind, length(yrange), ...
                     xval, yval ) );
      end
      
    else
      curval = oldresults.value(oldcommon(1));
    end

    results.values( yind, xind ) = curval;
  
  end
end

