// parameters
function y=J(x)
  y=sum(x.*x-cos(2*%pi*x))+length(x)  // Rastrigin
//  y=100*(x(2)-x(1)^2)^2+(x(1)-1)^2  // Rosenbrock
endfunction
n=2;xmin=-3;xmax=5;Vmax=1;
Npop=10;w=0.7;c1=0.7;c2=0.7;
Ngen=20;
//
//initialisation
PSO=zeros(Npop,3*n+1);       // size Npop*(3n)
PSO(:,1:n)=xmin+(xmax-xmin)*rand(Npop,n);
PSO(:,(n+1:2*n))=-Vmax+2*Vmax*rand(Npop,n);
PSO(:,(2*n+1:3*n))=PSO(:,1:n);
PSO(:,3*n+1)=1E10;
pg=zeros(1,n+1);pg(n+1)=1E10;pgval=[];
//
figure(1)
clf()
//
xmin=-5;xmax=5;N=100;
x=xmin:((xmax-xmin)/(N-1)):xmax;
y=x;
z=zeros(N,N);
for i=1:N
for j=1:N
z(i,j)=J([x(i),y(j)]);
end
end
//
for i=1:Ngen
  val=[];
  for j=1:Npop
    valj=J(PSO(j,1:n))
    val=[val;valj] // the evaluation of the swarm
    // computation of the new pi
    if (valj<PSO(j,3*n+1)) then
      PSO(j,2*n+1:3*n)=PSO(j,1:n);
      PSO(j,3*n+1)=valj;
    end 
  end
  // computation of pg
  [a,b]=gsort(val);ind=b($);
  best=[PSO(ind,1:n),val(ind)];
  if(val(ind)<pg(n+1)) then 
    pg=best;
  end
 // disp(pg);
  pgval=[pgval,pg(n+1)];
  // new velocitiy
PSO(:,(n+1:2*n))=w*PSO(:,(n+1:2*n))+c1*rand(Npop,n).*(PSO(:,2*n+1:3*n)-PSO(:,1:n))+c2*rand(Npop,n).*(ones(Npop,1)*pg(1:n)-PSO(:,1:n));
  // new value of x
PSO(:,1:n)=PSO(:,1:n)+PSO(:,(n+1:2*n)); 
    drawlater
    clf()
    plot2d(PSO(:,1),PSO(:,2),-1,rect=[-3,-3,5,5])
    contour2d(x,y,z,[0:0.01:0.1,0.2:1,1:30],rect=[-3,-3,5,5]);
//
    drawnow
end
disp('best elemet found for Rastrigin function (position+value):');disp(pg)
disp('number of evaluations:');disp(Ngen*Npop)

figure(2)
clf()
plot(1:Ngen,pgval)

