%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Nelder/Mead Simplex Method for Direct Search
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Kevin Passino
%   Version: 5/4/99
% 
% This program simulates the minimization of a simple function with
% the Nelder/Mead simplex method.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                % Initialize memory
figure(3) 
clf
figure(4) 
clf

p=2;                 % Dimension of the search space (p+1=number of vertices)

Nds=60;            % Maximum number of iterations to produce
beta=1;				 % Reflection coefficient (standard choice)
gamma=1;		     % Expansion coefficient (standard choice)
alpha=0.5;			 % Contraction coefficient (standard choice)

% Define the initial simplex vertices:

% With the following initialization, finds the global minimum at (15,5)
%P(:,:,1)=[0 15 30;
%          0 30 0];

% With the following initialization, finds global minimum at (15,5)
%P(:,:,1)=[0 30 30;
%          15 0 30];

% With the following initialization, finds global minimum at (15,5)
%P(:,:,1)=[0 0 30;
%          0 30 15];

% With the following initialization, finds local minimum at (20,15)
% since it starts pretty close to the local minimum
%P(:,:,1)=[0 15 30;
%          30 0 30];

% With the following initialization, finds global minimum at (15,5)
% since it is a good guess in the first place
P(:,:,1)=[20 15 15;
          5 0 15];

% With the following initialization, finds local minimum at (20,15) since
% it starts close to it
%P(:,:,1)=[15 20 15;
%          15 15 20];
		  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 and its contour map

figure(1)
clf
surf(x,y,z);
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');
zlabel('z=J');
title('Function to be minimized');

figure(2) 
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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the main loop

for j=1:Nds

	% First, compute the values of all the vertices
	
	for i=1:p+1
		
J(i,j)=optexampfunction([P(1,i,j);P(2,i,j)]);

   end

% Find the worst and best vertex and their indices

[Jmax(j),maxvertex(j)]=max(J(:,j));
[Jmin(j),minvertex(j)]=min(J(:,j));

thetamax(:,j)=P(:,maxvertex(j),j);
thetamin(:,j)=P(:,minvertex(j),j);

% Find the centroid of all the points, except the worst one

thetacent(:,j)=0*ones(p,1);  % Initialize it

for i=1:p+1
	
  thetacent(:,j)=thetacent(:,j)+P(:,i,j);

end

thetacent(:,j)=(1/p)*(thetacent(:,j)-thetamax(:,j));

% Reflection point computation step:

thetaref(:,j)=thetacent(:,j)+beta*(thetacent(:,j)-thetamax(:,j));

Jref(j)=optexampfunction([thetaref(1,j);thetaref(2,j)]);

% Next compute some values for the inequality tests in the reflection step

temp1=J(maxvertex(j),j); % Save the value of the cost at the worst vertex
J(maxvertex(j),j)=-Inf;  % Replaces the max element with a small element 
										% (for this problem then this element will not be 
										% chosen as the max)
[Jmaxwm(j),maxvertexwm(j)]=max(J(:,j)); % Finds the second largest element
J(maxvertex(j),j)=temp1; % Returns the matrix to how it was

% Next perform the inequality tests in the reflection step and then each 
% of the corresponding steps that they indicate.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For the first test in the reflection step

if Jmin(j)>Jref(j),
	% Then perform expansion
	
	thetaexp(:,j)=thetaref(:,j)+gamma*(thetaref(:,j)-thetacent(:,j));

	% Next, we have to compute Jexp, the cost at the expansion point
	
	Jexp(j)=optexampfunction([thetaexp(1,j);thetaexp(2,j)]);
 
   % Then, we attempt expansion
   
	if Jexp(j)<Jref(j),
		thetanew(:,j)=thetaexp(:,j);
		Jnew(j)=Jexp(j);
	else
		thetanew(:,j)=thetaref(:,j);
		Jnew(j)=Jref(j);
	end

	
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For the second test in reflection step

if Jmaxwm(j)>Jref(j) & Jref(j)>=Jmin(j),   
	% Reflection point has an intermediate value
	% so use the "use reflection step"
	
	thetanew(:,j)=thetaref(:,j);
	Jnew(j)=Jref(j);
	
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For the third test in reflection step

if Jref(j)>=Jmaxwm(j), 
	% The reflection point is not good
	
	% We perform contraction step
	
	if Jmax(j)<=Jref(j)
		thetanew(:,j)=alpha*thetamax(:,j)+(1-alpha)*thetacent(:,j);
	else
		thetanew(:,j)=alpha*thetaref(:,j)+(1-alpha)*thetacent(:,j);
	end
	
% We need Jnew for shrinking
	
	Jnew(j)=optexampfunction([thetanew(1,j);thetanew(2,j)]);

end

% Form the new simplex (use shrinking if Jnew in contraction shows 
% that thetanew is worse than the worst vertex):

if Jref(j)>=Jmaxwm(j) & Jnew(j)>Jmax(j), 
				% Tests that came from contraction, and
	            % that the new vertex is not good
		for i=1:p+1
			P(:,i,j+1)=0.5*(P(:,i,j)+thetamin(:,j));  % Shrink towards best vertex
		end
else 
		% Form the simplex in the usual way if you got here via
	    % any step except the the case where shrinking was already used	
	    P(:,:,j+1)=P(:,:,j);
	    P(:,maxvertex(j),j+1)=thetanew(:,j);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The next part is for plotting the simplex at each step to illustrate the 
% operation of the method

if j<=4,
	
figure(3) 
subplot(2,2,j)
contour(x,y,z,25)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
colormap(gray);
T=num2str(j-1);
T=strcat('Iteration j=',T);
ylabel(T)

hold on

% Next, add on the simplex at iteration j

simplexplot=plot([P(1,:,j) P(1,:,j)],[P(2,:,j) P(2,:,j)],'bo-');
set(simplexplot,'LineWidth',2);

hold off
%pause
end

%%%%%

if j>4 & j<=8,
	
figure(4) 
subplot(2,2,j-4)
contour(x,y,z,25)
colormap(jet)
% Use next line for generating plots to put in black and white documents.
colormap(gray);
T=num2str(j-1);
T=strcat('Iteration j=',T);
ylabel(T)

hold on

% Next, add on the the simplex at iteration j

simplexplot=plot([P(1,:,j) P(1,:,j)],[P(2,:,j) P(2,:,j)],'bo-');
set(simplexplot,'LineWidth',2);

hold off
%pause
end

end % End main loop...

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

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

figure(5) 
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

% Next, add on the vertices of the simplexes that were generated

for i=1:p+1,
	xx=squeeze(P(1,i,:));
	yy=squeeze(P(2,i,:));
	vertexplot=plot(xx,yy,'k.');
	set(vertexplot,'MarkerSize',10);
end

hold off

% Next, plot some data on the operation of the 

figure (6)
clf
subplot(121)
plot(t,thetamin(1,:),'k-',t,thetamin(2,:),'k--')
xlabel('Iteration, j')
ylabel('Vertex position')
title('Vertex of minimum cost')

subplot(122)
plot(t,Jmin,'k-',t,Jmax,'k--')
xlabel('Iteration, j')
ylabel('Cost J')
title('Cost of best and worst vertex')

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