%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Response surface methodology for studying
% instinct-learning balance
%
% By: Kevin Passino
% Version: 5/2/01
%
% Note: This code can take a relatively long
% time to run (of course depending on 
% what computer you are using).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all             % Initialize memory


NL=200; % Set the number of steps in the lifetime of the organism
time=1:NL; % Time steps during a lifetime

Ng=100; % Set the number of generations
timeepoch=1:Ng; 

% Set environmental characteristics

y=zeros(NL,Ng); % Initialize/allocate memory for sensed variable from environment
z=zeros(NL,Ng); % Initialize
sigmaz2=0.5*ones(NL,Ng);  % Initialize the variance in the variable we want to estimate (could
                      % change during a lifetime or over generation)

xmin=0; % Set limits on size of x values
xmax=4;

xbar=2; % For case where have an unknown constant in the environment that want to estimate
x=xbar*ones(NL,Ng); % Set variable want to estimate as a constant
xhat=zeros(NL,Ng);

Ndivxhat0=40; % Set the number of points on a xbar=xhat0 axis
xhat0=linspace(xmin,xmax,Ndivxhat0); % Initialize the estimator initial conditions 

Ndivn=20; % Number of divisions on n space, same as the max value considered for n
n=1:Ndivn; 

% Initialize cost function

J=zeros(Ndivn,Ndivxhat0,Ng);

% Set weighting parameters for cost function

% A choice to illustrate some concepts:
w1=0.01;
w2=1; 
w3=0.05;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start loop for generation of data for response surfaces
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


for ell=1:Ng % Loop for testing multiple times...
	
for ii=1:length(xhat0)
	for jj=1:length(n)
	
	clear Y  % Dimension changes on the regressor vector so clear it each time it changes
	Y=xhat0(ii)*ones(n(jj),1); % Set initial condition
	
	% Run estimator for lifetime of animal
	for k=1:NL  % Generate sensed signals, including initial values that are instincts
		z(k,ell)=sigmaz2(k,ell)*randn; % Generate noise
		y(k,ell)=x(k,ell)+z(k,ell); % Generate signal that is sensed
		Y=[y(k,ell); Y(1:(n(jj)-1),1)]; % Shift regression vector, load in new value
		xhat(k,ell)=mean(Y);
	end

    J(jj,ii,ell)=w1*n(jj)... % Cost of storage
            +w2*(1/NL)*(x(:,ell)-xhat(:,ell))'*(x(:,ell)-xhat(:,ell))... % Quality of estimation
			+w3*exp(-(x(1,ell)-xhat0(ii))^2/((0.1)^2)); % Models cost of instinct
    end % End ii loop
end % End jj loop


end % End ell loop

% Compute average of performance measure for Ng runs

for ii=1:length(xhat0)
	for jj=1:length(n)
		Javg(jj,ii)=mean(J(jj,ii,:)); % Compute average of cost measure across the generations
	end
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[val,rowindex]=min(min(Javg)); % Gives the min value (val) and its row index
[val,colindex]=min(min(Javg')); % Gives the min value and its column index
% Best point

bestpoint=[xhat0(rowindex); n(colindex)]

figure(1)
clf
surf(xhat0,n,Javg)
view(-162,48)
colormap(jet)
colormap(white);
hold on
bestpointplot=plot3(bestpoint(1,1),bestpoint(2,1),Javg(colindex,rowindex),'r.');
set(bestpointplot,'MarkerSize',20);
hold off
title('Average cost')
xlabel('Initial condition for estimate (x=2)')
ylabel('n')
rotate3d on

figure(2)
clf
for ii=1:length(xhat0)
for jj=1:length(n)
plot(timeepoch,squeeze(J(jj,ii,:)),'k-')
hold on
end
end
hold off
title('Cost')
xlabel('Generation')
ylabel('J')

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