%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Torczon's multidirectional search algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Kevin Passino
%   Version: 3/21/00
% 
% This program simulates the minimization of a simple function with
% the multidirectional search method.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                % Initialize memory

p=2;                 % Dimension of the search space 

Nmds=200;            % Maximum number of iterations to produce

% Next set the parameters of the algorithm:

gammae=2;
gammac=1/2;

% Set the parameter for the stopping criterion (based on how much the minimum
% cost vertex has changed from one iteration to the next, after one iteration)

epsilon=0.0000000001;
stopflag=0;  % Used to signal termination criterion has been met
		  

% Initialize to avoid use of memory manager:

J=0*ones(p+1,1);
Jrot=0*ones(p,1);
Jexp=0*ones(p,1);
Jcont=0*ones(p,1);

thetarot=0*ones(p,p);
thetaexp=0*ones(p,p);
thetacont=0*ones(p,p);

P=0*ones(p,p+1,Nmds+1); % Max possible used  - stopping criteria may be invoked

% Next, pick the initial simplex

% With next initialization it falls into the local minimum at (20,15) and the
% stopping criterion is used (as set above)
P(:,:,1)=[15.2 14.9 16;
          20.6 20.1 19.3];

% With next initialization it falls into the upper left corner local minimum and the
% stopping criterion is used (as set above)
%P(:,:,1)=[16 14 16;
%          21 22 19];

% With next initialization it falls into the global minimum at (15,5) and the
% stopping criterion is used (as set above)
%P(:,:,1)=[16 15 14;
%          4 7 3];

% With next initialization it falls into the global minimum at (15,5)  and the
% stopping criterion is used (as set above)
%P(:,:,1)=[30 0 15;
%          0 0 30];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the MDS loop

for j=1:Nmds

	if stopflag==1
		break
	end
	
	% Step 1: find min cost index and place it at P(:,1,j) (first column)
	
for i=1:p+1			% This is not the simplest way computationally - for j>1
					% this approach results in recomputation of costs that
					% were already computed below in rotation, expansion, or contraction
					% Just do it this way for programming simplicity (laziness on the author's part)
	
	J(i,1)=optexampfunction([P(1,i,j);P(2,i,j)]);

end

	[Jmin,istar]=min(J(:,1)');  % Find the index istar of the minimum cost
		
	firstcolumn=P(:,1,j);  % Swap the first column (vertex 0) with the best vertex
	P(:,1,j)=P(:,istar,j);
	P(:,istar,j)=firstcolumn;
	
	minvertex_notreplaced=1;  % =1 represents that the minimum vertex has not been 
								% replaced yet (and steps 2,3,4 continue till it is)
								
	while minvertex_notreplaced==1

		% Need stopping criterion here (based on change in size of best vertex)
				
		if j>1 
			% Compute min vertex change amount
			change=(P(:,1,j)-P(:,1,j-1))'*(P(:,1,j)-P(:,1,j-1));
			if change<epsilon*(P(:,1,j-1)'*P(:,1,j-1))
				stopflag=1;
			end
		end
			
	% Step 2: Rotation step
			
	for i=1:p
		
	thetarot(:,i)=P(:,1,j)-(P(:,i+1,j)-P(:,1,j)); % The +1 is due to 1 representing 0
	
	Jrot(i,1)=optexampfunction([thetarot(1,i);thetarot(2,i)]);

	end

	% Step 3: Expansion step
	
	[Jrotmin,istarrot]=min(Jrot(:,1)');
	
	if Jrotmin<Jmin
		
		minvertex_notreplaced=0; % Found a point with a cost better than minimum one 
									% of current simplex
									
		for i=1:p
			
		thetaexp(:,i)=P(:,1,j)-gammae*(P(:,i+1,j)-P(:,1,j)); % The +1 is due to 1 representing 0

		Jexp(i,1)=optexampfunction([thetaexp(1,i);thetaexp(2,i)]);
   
        end
	  
		  	[Jexpmin,istarexp]=min(Jexp(:,1)');

		if Jexpmin<Jrotmin
			
		minvertex_notreplaced=0; % Found a point with a cost better than minimum one 
									% of current simplex

			P(:,1,j+1)=P(:,1,j); % Keep the best point from the last iteration
			
			for i=1:p
				P(:,i+1,j+1)=thetaexp(:,i); % Form new simplex with expansion
			end
			
		else
			
			P(:,1,j+1)=P(:,1,j); % Keep the best point from the last iteration
			
			for i=1:p
				P(:,i+1,j+1)=thetarot(:,i); % Form new simplex with rotation
			end
			
		end
	
	else % For  if Jrotmin<Jmin above

		% Step 4: Contraction step
		
			for i=1:p
			
		thetacont(:,i)=P(:,1,j)+gammac*(P(:,i+1,j)-P(:,1,j)); % The +1 is due to 1 representing 0

		Jcont(i,1)=optexampfunction([thetacont(1,i);thetacont(2,i)]);
   
        end
	
	
		  	[Jcontmin,istarcont]=min(Jcont(:,1)');
				
			P(:,1,j+1)=P(:,1,j); % Keep the best point from the last iteration
			
			for i=1:p
				P(:,i+1,j+1)=thetacont(:,i); % Form new simplex with contraction
			end
					
			if Jcontmin<Jmin
						
				minvertex_notreplaced=0; % Found a point with a cost better than minimum one 
									% of current simplex
			else
									
				P(:,:,j)=P(:,:,j+1); % In this case we take the progress made by the 
										% the contraction and restart the inner loop with it

			end
		
	end % For  if Jrotmin<Jmin above
		
	end % for while loop

end % End main loop...




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the function we are seeking the minimum of:

x=0:31/100:30;   % For our function the range of values we are considering
y=x;

% Compute the function that we are trying to find the minimum of.

for jj=1:length(x)
	for ii=1:length(y)
		z(ii,jj)=optexampfunction([x(jj);y(ii)]);
	end
end

% First, show the actual function to be maximized

figure(1)
clf
surf(x,y,z);
colormap(jet)
% Use next line for generating plots to put in black and white documents.
%colormap(white);
xlabel('x=\theta_1');
ylabel('y=\theta_2');
zlabel('z=J');
title('Function to be minimized');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Next, provide some plots of the results of the simulation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


t=0:j-1;  % For use in plotting

figure(2) 
clf
plot(t,squeeze(P(1,1,1:j)),'k-',t,squeeze(P(2,1,1:j)),'k--')
xlabel('Iteration, j')
ylabel('\theta_1, \theta_2')
title('MDS best vertex trajectories')


figure(3) 
clf
contour(x,y,z,25)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
colormap(gray);
xlabel('x=\theta_1');
ylabel('y=\theta_2');
title('Function to be minimized (contour map)');

hold on

plot(squeeze(P(1,1,1:j)),squeeze(P(2,1,1:j)),'k-')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

