%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Simple coordinate search algorithm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Kevin Passino
%   Version: 4/6/00
% 
% This program simulates the minimization of a simple function with
% the simple coordinate search method.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                % Initialize memory

p=2;                 % Dimension of the search space 

Ncs=200;            % Maximum number of iterations to produce

% Specify the pattern:

C=[eye(p,p) -eye(p,p) 0*ones(p,1) ];
[temp,sizeC]=size(C); % Specify the number of columns as the number of elements of C

% Next set the parameters of the algorithm:

gammac=1/2;
lambda=0*ones(1,Ncs+1);
lambda(1,1)=1;

% Set the parameter for the stopping criterion (based on small lambda has become)

epsilon=0.0001;		  

% Initialize to avoid use of memory manager:

J=0*ones(sizeC,1);
thetasbest=0*ones(p,1);
thetas=0*ones(p,sizeC-1);

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

% Next, pick the initial set

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


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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the coordinate search loop

for j=1:Ncs

% Step 2: Compute cost at current estimate (note that second time around loop 
% actually know this value, but recompute it - you could make this more efficient):

J(1,1)=optexampfunction([P(1,1,j);P(2,1,j)]);
			
% Step 3: Exploratory moves step

% Step 3 (a):

thetasbest=0*ones(p,1);
rho=0;
Jmin=J(1,1);

% Step 3 (b):

	for i=1:sizeC-1
		
		thetas(:,i)=lambda(1,j)*C(:,i);  % Define perturbation for pattern point
		P(:,i+1,j)=P(:,1,j)+thetas(:,i); % Compute the pattern point by perturbing from 
		                                 % the from the center point
		
		J(i+1,1)=optexampfunction([P(1,i+1,j);P(2,i+1,j)]); % Find cost at the pattern point
		
	if J(i+1,1)<Jmin  % If find a patter point that is better than the middle one
		rho=J(1,1)-J(i+1,1); % Define how much cost reduction was achieved
		Jmin=J(i+1,1); % Found a better cost so save it
		thetasbest=thetas(:,i); % Save the best point found so far
	end
	
	end
		
% Step 4: Update pattern center and decide if contraction is needed

if rho>0
	P(:,1,j+1)=P(:,1,j)+thetasbest;  % Found a better point so take it
	lambda(1,j+1)=lambda(1,j);           % and do not contract
else
	P(:,1,j+1)=P(:,1,j);  % Did not find a better point so use old one and contract
	lambda(1,j+1)=gammac*lambda(1,j);
end

% Stopping criterion: 
				
		if lambda(1,j)<epsilon
			break;
		end
	
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('Simple coordinate search 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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%