function [OUT,varargout]=pso(functname,D,varargin)

% PSO.M
% a generic particle swarm optimizer
% to find the minimum or maximum of any 
% MISO matlab function
%
% Brian Birge
% Rev 1.0
% 1/1/3
%
% Usage:
%    [optOUT]=PSO(functname,D)
%   or:
%    [optOUT,tr,te]=PSO(functname,D,VarRange,minmax,PSOparams)
%
% Inputs:
%    functname - string of matlab function to optimize
%    D - # of inputs to the function (dimension of problem)
%
% Optional Inputs:
%    VarRange - matrix of ranges for each input variable, default -100 to 100, of form:
%       [ min1 max1 
%         min2 max2
%            ...
%         minD maxD ]
%
%    minmax - if 0 then funct is minimized, if 1 then funct maximized, default=0
%
%    PSOparams - PSO parameters
%      P(1) - Epochs between updating display, works with P(13), default = 25.
%      P(2) - Maximum number of iterations (epochs) to train, default = 2000.
%      P(3) - population size, default = 20
%      P(4) - maximum particle velocity, default = 4
%      P(5) - acceleration const 1 (local best influence), default = 2
%      P(6) - acceleration const 2 (global best influence), default = 2
%      P(7) - Initial inertia weight, default = 0.9
%      P(8) - Final inertia weight, default = 0.2
%      P(9)- Iteration (epoch) by which inertial weight should be at final value, default = 1500
%      P(10)- randomization flag (flagg), for PSO conforming to literature = 2, default = 2:
%                flagg = 0, same random numbers used for each particle (different at each epoch - least randomness)
%                flagg = 1, separate randomized numbers for each particle at each epoch
%                flagg = 2, separate random #'s at each component of each particle at each epoch (most randomness)
%      P(11)- minimum global error gradient, if abs(Gbest(i+1)-Gbest(i)) < gradient over 
%                 certain length of epochs, terminate run, default = 1e-9
%      P(12)- epochs before error gradient criterion terminates run, default = 50
%                 i.e., if the SSE does not change over 50 epochs, quit program
%      P(13)- plot flag, shows progress display if =1, nothing otherwise, default = 1
%
% Outputs:
%    optOUT - optimal inputs and associated min/max output of function, of form:
%        [ bestin1
%          bestin2
%            ...
%          bestinD
%          bestOUT ]
%
% Optional Outputs:
%    tr    - Gbest at every iteration, traces flight of swarm
%    te    - epochs to train, returned as a vector 1:endepoch
%
% Example:  out=pso('f6',2)
%
% See Also: TRAINPSO
close all

rand('state',sum(100*clock));
if nargin < 2
   error('Not enough arguments.');
end

% PSO PARAMETERS
if nargin == 2
   VRmin=ones(D,1)*-100; 
   VRmax=ones(D,1)*100;    
   VR=[VRmin,VRmax];
   minmax = 0;
   P = [];
elseif nargin == 3
   VR=varargin{1};
   minmax = 0;
   P = [];
elseif nargin == 4
   VR=varargin{1};
   minmax=varargin{2};
   P = [];
elseif nargin == 5
   VR=varargin{1};
   minmax=varargin{2};
   P = varargin{3};
end

P = nndef(P,[25 2000 20 4 2 2 0.9 0.2 1500 2 1e-9 50 1]);
df  = P(1);
me  = P(2);
ps  = P(3);
mv  = P(4);
ac1 = P(5);
ac2 = P(6);
iw1 = P(7);
iw2 = P(8);
iwe = P(9);
flagg=P(10);
ergrd=P(11);
ergrdep=P(12);
plotflg=P(13);

% PLOTTING
 message = sprintf('PSO: %%g/%g iterations, GBest = %%g.\n',me);

 
%------------------------------------------------------------------------------------------------------------------- 
% initialize population of particles and their velocities at time zero, format of pos= (particle#, dimension, epoch)
 for dimcnt=1:D
  pos(1:ps,dimcnt)=normalize(rand([ps,1]),VR(dimcnt,:),0); % construct random population positions bounded by VR
  vel(1:ps,dimcnt)=normalize(rand([ps,1]),[-mv,mv],0); % construct initial random velocities between -mv,mv
 end

% initial pbest positions vals
 pbest=pos;
 
 for j=1:ps  % start particle loop
    numin='0';
    for i=1:D
        numin=strcat(numin,',',num2str(pos(j,i)));
    end
    evstrg=strcat('feval(''',functname,'''',numin(2:end),')');
    out(j)=eval(evstrg);     % evaluate desired function with particle j      
 end
 
 pbestval=out;   % initially, pbest is same as pos

% assign initial gbest here also (gbest and gbestval)
 if minmax==1
    [gbestval,idx1]=max(pbestval);  % this picks gbestval when we want to maximize the function
 elseif minmax==0
    [gbestval,idx1]=min(pbestval);  % this works for straight minimization
 end
 gbest=pbest(idx1,:);  % this is gbest position
 tr(1)=gbestval;       % save for output

% start PSO iterative procedures
cnt=0; % counter used for updating display according to df in the options
cnt2=0; % counter used for the stopping subroutine based on error convergence


for i=1:me  % start epoch loop (iterations)
   if flagg==0   % randimization control, one random set for each epoch
       rannum1=rand(1);  
       rannum2=rand(1);
   end
   
   for j=1:ps  % start particle loop
       
     if flagg==1   % randomization control, one random set for each particle at each epoch
         rannum1=rand(1);
         rannum2=rand(2);
     end

     numin='0';
     for dimcnt=1:D
         numin=strcat(numin,',',num2str(pos(j,dimcnt)));
     end
     evstrg=strcat('feval(''',functname,'''',numin(2:end),')');
     out(j)=eval(evstrg);     % evaluate desired function with particle j  

     e(j) = out(j);              % use to minimize or maximize function to unknown values

     %SSEhist(j) = sumsqr(e);    % sum squared 'error' for jth particle (averages if there is more than one output)
     
    % update pbest to reflect whether searching for max or min of function
     if minmax==0
       if pbestval(j)>=e(j);
          pbestval(j)=e(j);
          pbest(j,:)=pos(j,:);
       end
     elseif minmax==1
       if pbestval(j)<=e(j);
           pbestval(j)=e(j);
           pbest(j,:)=pos(j,:);
       end
     end
 
          
    % assign gbest by finding minimum of all particle pbests 
     if minmax==1
       [iterbestval,idx1]=max(pbestval);  % this picks gbestval when we want to maximize the function
       if gbestval<=iterbestval
          gbestval=iterbestval;
          gbest=pbest(idx1,:);
       end       
     elseif minmax==0 
       [iterbestval,idx1]=min(pbestval);  % this works for straight minimization and for minimizing error to target
       if gbestval>=iterbestval
          gbestval=iterbestval;
          gbest=pbest(idx1,:);
       end       
     end    
    
     tr(i+1)=gbestval; % keep track of global best val
     te=i;           % this will return the epoch number to calling program when done

    % get new velocities, positions (this is the heart of the PSO algorithm)
     if i<=iwe
        iwt(i)=((iw2-iw1)/(iwe-1))*(i-1)+iw1; % get inertia weight, just a linear funct w.r.t. epoch parameter iwe
     else
        iwt(i)=iw2;
     end
    
     if flagg==2              % each component of each particle gets a different random number set
        for dimcnt=1:D
           rannum1=rand(1);
           rannum2=rand(1);
           vel(j,dimcnt)=iwt(i)*vel(j,dimcnt)...
                    +ac1*rannum1*(pbest(j,dimcnt)-pos(j,dimcnt))...
                    +ac2*rannum2*(gbest(1,dimcnt)-pos(j,dimcnt));
        end
     else                     % this velocity update is for flagg= 0 or 1
        vel(j,:)=iwt(i)*vel(j,:)...
                 +ac1*rannum1*(pbest(j,:)-pos(j,:))...
                 +ac2*rannum2*(gbest(1,:)-pos(j,:));
     end

    % update new position
     pos(j,:)=pos(j,:)+vel(j,:);    

    % limit velocity/position components to maximums
     for dimcnt=1:D
        if vel(j,dimcnt)>mv
           vel(j,dimcnt)=mv;
        end
       
        if vel(j,dimcnt)<-mv
          vel(j,dimcnt)=-mv;
        end    
        
        if pos(j,dimcnt)>=VR(dimcnt,2)
           pos(j,dimcnt)=VR(dimcnt,2);
        end
       
        if pos(j,dimcnt)<=VR(dimcnt,1)
           pos(j,dimcnt)=VR(dimcnt,1);
        end       
        
        
     end
       
   end         % end particle loop
   
  %---------------------------------------------------------------------------------------------------------------------- 
   
   
   % check for stopping criterion based on speed of convergence to desired error   
    tmp1=abs(tr(i)-gbestval);
    if tmp1>ergrd
       cnt2=0;
    elseif tmp1<=ergrd
       cnt2=cnt2+1;
       if cnt2>=ergrdep
         if plotflg==1
          fprintf(message,i,gbestval);           
          disp(' ');
          disp('***** global error gradient too small for too long');
          disp('***** this means you''ve got the solution or it got stuck');
         end
         break
       end       
    end
   
if plotflg==1
  iii=i+df-1; % quick mod so it displays on the 1st epoch too
      
  if rem(iii,df) == 0 
     fprintf(message,i,gbestval);
    % pretty plots
      if D==1     
        subplot(2,1,1)          
          stem([pos(:,1);gbest],'r');
          hold on
          stem(pos(:,1),'b');
          grid on
          hold off
          xlabel('particle #');
          ylabel('position');
          drawnow
        subplot(2,1,2)          
      elseif D==2
        subplot(2,1,1)
          plot(pos(:,1),pos(:,2),'b.');
          hold on
          plot(gbest(1,1)',gbest(1,2)','r*'); 
      %    axis([VR(1,1),VR(1,2),VR(2,1),VR(2,2)]);
          grid on
          hold off
          xlabel('Pos dimension 1');
          ylabel('Pos dimension 2');
          drawnow
        subplot(2,1,2)          
      elseif D==3
        subplot(2,1,1)    
          plot3(pos(:,1),pos(:,2),pos(:,3),'b.');
          hold on
          plot3(gbest(1,1)',gbest(1,2)',gbest(1,3),'r*'); 
          axis([VR(1,1),VR(1,2),VR(2,1),VR(2,2),VR(3,1),VR(3,2)]);
          grid on
          hold off
          xlabel('Pos dimension 1');
          ylabel('Pos dimension 2');
          zlabel('Pos dimension 3');
          drawnow
        subplot(2,1,2)          
      else

      end

      semilogy([0,1:te],tr);
      xlabel('epoch');
      ylabel('Gbest value');
      title(['PSO: ',num2str(D),' dimensional prob search, Gbestval=',num2str(gbestval)]);      
      drawnow     
  end  % end update display every df if statement    
 end % end plotflg if statement
end  % end epoch loop

% outputs
 OUT=[gbest';gbestval];
 varargout{1}=[0:te];
 varargout{2}=[tr];