%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% SPSA for Attentional Strategy Design
%
% By: Kevin Passino
% Version: 4/28/01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all             % Initialize memory

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Characteristics of the predator/prey environment and organism:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global Tmax Tsample Nsteps N

Tmax=200;  					% Length of simulation time (real time, in seconds)
Tsample=0.01; 				% Sampling period (used to discretize)
Nsteps=(1/Tsample)*Tmax;  	% Number of steps to run the simulation
N=4;         				% Number of predators/prey (note that increasing this may
							% require increases in Tmax since we need for the performance
							% evaluation to reach some type of "steady state" for it to 
							% adequately represent the J value below)
%-------------------------
% Predator/prey characteristics:
%-------------------------

global f deltai

% Define predators/prey appearance instants (note that f(i,1) refers to predator/prey i, and that predator/prey i
% has priority i, where higher values of i indicate higher priority predators/prey)

f(:,1)=0*ones(N,1); % Allocate memory
f(1,1)=1*(1/Tsample);  % Sets that we want an appearance from predator/prey 1 every f(1,1) steps (1.2 sec.)
						 % (this sets the frequency of appearance)
f(2,1)=1.1*(1/Tsample);  
f(3,1)=1.2*(1/Tsample);  
f(4,1)=1.3*(1/Tsample);  
%f(5,1)=1.4*(1/Tsample);


% Next, define the bounds delta^i on the maximum amount of time between the appearances of each predator/prey
% (note that these must be consistent with the choice for the f(;,1) parameters above that define when the 
% appearances occur).  These are given in real time, not steps.

deltai(1,1)=1.05; % Overbounds a choice of 1.2 above
deltai(2,1)=1.15; 
deltai(3,1)=1.25;
deltai(4,1)=1.35;
%deltai(5,1)=1.45;


%---------------------------
% Organism characteristics:
%---------------------------

global sched a deltas

sched=4; % Chooses the policy of choosing the predators/prey that may be most difficult to detect
% For sched=4, need weighting factors, which are the parameters of the optimization problem.
% To initialize these weights, choose them all to be the same

% Choose initialization for w below

% Relatively overloaded organism; sum=0.7 (also gives differring rates of processing for 
% predators/prey with different delay characteristics)
a(1,1)=0.1;
a(2,1)=0.2;
a(3,1)=0.3;
a(4,1)=0.1;

% Set the a_i parameters so that the capacity condition is met:
%epsilon=0.1;                    % Used to define a_i parameters next
%a(:,1)=epsilon*(1/N)*ones(N,1); % Picks so will meet capacity condition - scale it 
                                      % (other choices are possible)

deltas=3*Tsample; % Sets the delay (in steps) that it takes before the organism can 
                 % switch from focusing on one predator/prey to another.
				 % NOTE: This only accounts for part of the "delta" used in the book.
				 % This is the part due to switching from one predator/prey to another.  


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SPSA parameters:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p=N;                 % Dimension of the search space  (here, equal to the number of 
					% predators/prey since there is one parameter for each in the 
					% attentional strategy

thetamin=0.1*ones(p,1);  % Set edges of region want to search in (for w values above)
thetamax=10*ones(p,1);

Nspsa=100;            % Maximum number of iterations to produce

% Next set the parameters of the algorithm:

lambda=0.5;
lambda0=10;

% However, values $\alpha_{1}\in [0.602,1)$ and $\alpha_{2} \in [ 0.101,
%\frac{1}{6}]$ may work also.  In fact $\alpha_{1}=1$ and 
%$\alpha_{2}=\frac{1}{6}=0.166667$ are the ``asymptotically optimal'' values so 
%if the algorithm runs for a long time it may be beneficial to switch 
%to these values. 

alpha1=0.602;
alpha2=0.101;
%alpha1=0.9; % Some tuned values
%alpha2=0.16;

c=0.25; % Sets size of perturbations
		  
% Next, pick the initial value of the parameter vector

%theta(:,1)=[4; 2; 1; 4];

theta(:,1)=ones(p,1);  % Sets equal weighting to all the predators/prey


% Allocate memory 

theta(:,2:Nspsa)=0*ones(p,Nspsa-1);
J=0*ones(Nspsa,1);  % Cost function for SPSA
cj=0*ones(Nspsa,1);  % Initialize some SPSA parameters, c and lambda
lambdaj=0*ones(Nspsa,1); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the SPSA loop

for j=1:Nspsa
 
	% Use projection in case update (or initial values) out of range
	
	theta(:,j)=min(theta(:,j),thetamax);
	theta(:,j)=max(theta(:,j),thetamin);

	% For use in plotting ONLY - just to illustrate how cost decreases are obtained at 
	% the theta values - the SPSA DOES NOT use this cost evaluation to update parameters
	% as it only needs two cost evaluations per iteration, an no evaluation at the actual
	% parameter value - just at perturbed ones thetaplus and thetaminus below.
	
	[J(j,1),etemp,predpreyfocusedontemp,Ttemp,avgTtemp,maxTtemp]=attentionenv(theta(:,j));

	% Set parameters and perturb parameters
	
	lambdaj(j,1)=lambda/(lambda0+j)^alpha1;  % Note since this corresponds to zero
	cj(j,1)=c/j^alpha2;
	Delta=2*round(rand(p,1))-1; % According to a Bernoulli +- 1 dist.
	thetaplus=theta(:,j)+cj(j,1)*Delta;
	thetaminus=theta(:,j)-cj(j,1)*Delta;

	% Use projection in case perturbed out of range
	
	thetaplus=min(thetaplus,thetamax);
	thetaplus=max(thetaplus,thetamin);
	thetaminus=min(thetaminus,thetamax);
	thetaminus=max(thetaminus,thetamin);

	% Next we must compute the cost function for thetaplus and thetaminus
	
	[Jplus,e,predpreyfocusedon,T,avgT,maxT]=attentionenv(thetaplus);
	[Jminus,e,predpreyfocusedon,T,avgT,maxT]=attentionenv(thetaminus);
	
	% Next, compute the approximation to the gradient
	
	g=(Jplus-Jminus)./(2*cj(j,1)*Delta);
	
	% Then, update the parameters
	
	theta(:,j+1)=theta(:,j)-lambdaj(j,1)*g;
	
% The following code is used to determine how large to pick Tmax.  It must be big enough so that
% the J performance measure has reached a "steady state" and for larger N it can take longer to 
% converge.  Hence, the next few lines are used to plot the avgT vs k to see if looks like the
% average has converged. Of course with a little more work you could make it automatically test whether the 
% the simulation has run long enough

% So - just show for thetaminus

%time=0:Tsample:Tmax;

%figure(1)
%clf
%subplot(311)
%stairs(time,predpreyfocusedon)
%title('Predator/prey focused on')
%subplot(312)
%plot(time,avgT,time,(1/(Nsteps+1))*sum(avgT),'r.')
%title('Average length of time since last detect')
%subplot(313)
%plot(time,maxT,time,(1/(Nsteps+1))*sum(maxT),'r.')
%xlabel('Time, sec.')
%title('Maximum length of time since last detect')

%pause

end % End main loop...


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

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

figure(2) 
clf
plot(t,J(:,1),'k--')
ylabel('J')
xlabel('Iteration, j')
title('Cost for \theta(j)')
zoom

figure(3) 
clf
subplot(211)
plot(t,cj(:,1),'k--')
ylabel('c(j)')
title('SPSA parameters')
subplot(212)
plot(t,lambdaj(:,1),'k--')
ylabel('\lambda(j)')
xlabel('Iteration, j')

clear t
t=0:Nspsa;  % For use in plotting

figure(4) 
clf
for ii=1:p
plot(t,theta(ii,:))
hold on
end
ylabel('w_i parameters')
xlabel('Iteration, j')
Tend=num2str(max(t));
Tw1=num2str(theta(1,max(t)));
Tw2=num2str(theta(2,max(t)));
Tw3=num2str(theta(3,max(t)));
Tw4=num2str(theta(4,max(t)));
T=strcat('SPSA paramters, j=',Tend,': w_1=',Tw1,', w_2=',Tw2,', w_3=',Tw3,', w_4=',Tw4);
title(T)
hold off

% Print out some data to get the parameter values

[minvalavgavgT,indexofstep]=min(J(:,1))
theta(:,indexofstep)

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