% Particle Swarm Optimization
%
close all; drawnow; 
clear

addpath('../Chapter1');

ff = @(x) testfunction(x,12); % our objective function (must take 2 dimensional input)

% Initializing variables:
% 
popsize = 10; % size of the swam
npar = 2; % dimension of the problem
maxit = 100; % maximum number of iterations
c1 = 1; % cognitive parameter
c2 = 4 - c1; % social parameter 
C = 1; % constriction factor

% Intialization swarm and velocites:
par = rand(popsize,npar); % random population of continious values
vel = rand(popsize,npar); % random velocity

% Evaluate intial population: 
cost = feval(ff,par); % calculates population cost using ff
minc(1) = min(cost); % min cost
meanc(1) = mean(cost); % mean cost 
globalmin = minc(1); % initalize global minimum 

% Initialize local minimum for each particle 
localpar = par; % location of local minima
localcost = cost; % cost of local minima

% Finding best particle in initial population 
[globalcost,indx] = min(cost);
globalpar = par(indx,:);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start iterations
iter = 0; % counter 

while iter < maxit
  iter = iter + 1;

  % update velocity = vel
  w = (maxit - iter)/maxit; % inertia weiindxht
  r1 = rand(popsize,npar); % random numbers
  r2 = rand(popsize,npar); % random numbers
  vel = C*(w*vel + c1 * r1 .* ( localpar - par ) + c2*r2.*(ones(popsize,1)*globalpar-par));
  
 % updates particle positions:
  par = par + vel;
  overlimit = par <= 1;
  underlimit = par >= 0;
  par = par.*overlimit + not(overlimit);
  par = par.*underlimit;

  % evaluate the new swarm:
  cost = feval(ff,par); % evaluates cost of a swarm

  % update the best local position for each particle
  bettercost = cost < localcost;
  localcost = localcost .* not(bettercost) + cost.*bettercost;
  localpar(find(bettercost),:) = par(find(bettercost),:);

  % updating index g:
  [temp,t] = min(localcost);
  if temp < globalcost 
    globalpar = par(t,:); 
    indx = t;
    globalcost = temp; 
  end 

  [iter globalpar globalcost] % print output each iteration

  minc(iter+1) = min(cost); % min for iteration

  globalmin(iter+1) = globalcost; % best min so far
 
  meanc(iter+1) = mean(cost); % avg. cost for this iteration
end %while

figure(24);
iters = 0:length(minc)-1;
plot(iters,minc,iters,meanc,'-',iters,globalmin,':');
xlabel('generation'); ylabel('cost'); 
text(0,minc(1),'best'); text(1,minc(2),'population average');
%