// PSO
// Parametri:
//   m:  numarul de elemente 
//   n: dimensiunea fiecarui element
//   gen:  numar de generatii
//   [a,b]: domeniul de definitie al functiei de optimizat (minimizat)
//   gama: factor care multiplica "viteza"
//   r1: factor care multiplica diferenta dintre cea mai buna pozitie a unui
//       element si pozitia curenta
//   r2: factor care multiplica diferenta dintre cea mai buna pozitie din 
//       populatie si pozitia curenta
//   functie: numele functiei de minimizat
// Exemplu de apel:[xbest,fbest]=PSO(10,1,100,-50,50,0.1,0.5,0.5,@PSO_f1)

function [xBest,fbest]=PSO(m,n,gen,a,b,gama,r1,r2,functie)
x=initpop(m,n,a,b); //initializarea populatiei
best=x; // initializarea celei mai bune pozitii corespunzatoare fiecarui element al populatiei
v=zeros(m,n); // initializarea "vitezelor"
f=evalpop(functie,x);  // evaluarea populatiei initiale
fb=f;
indBest=findBest(f);   // determinarea celui mai bun element al populatiei
fbest=f(1,indBest); 
xBest=x(indBest,:);
for k=1:gen            // ciclul de evolutie
   for i=1:m           // determinarea noii pozitii pentru fiecare element al populatiei
      
      // calculul noii valori a vitezei 
      v(i,:)=gama*(v(i,:)+r1*grand(1,n,'unf',0,1).*(best(i,:)-x(i,:))+ r2*grand(1,n,'unf',0,1).*(xBest(1,:)-x(i,:)));
      // modificarea pozitiei
      x(i,:)=x(i,:)+v(i,:);
      // corectarea elementelor generate in afara domeniului
      x(i,:)=repair(x(i,:),a,b);
      // evaluarea noii pozitii
      //disp(x(i,:),"x=")
      f(1,i)=functie(x(i,:));
      // actualizarea celei mai bune pozitii corespunzatoare elementului i
      // (memoria personala)
      if f(1,i)<fb(1,i)
          best(i,:)=x(i,:);
          fb(1,i)=f(1,i);
      end    
      // actualizarea celei mai bune pozitii din populatie
      // (memoria colectiva - varianta cu caracter global)
      if f(1,i)<fbest
         indBest=i;
         fbest=f(1,i);
         xBest=x(indBest,:);
      end
      disp(fbest, 'fbest')
   end
end 
 // reprezentarea grafica a functiei si a celor mai bune pozitii
 // identificate de catre elementele populatiei
 if n==1
      xx=a:0.1:b; y=zeros(1,size(xx,2));
      for j=1:size(xx,2)
         y(j)=functie(xx(j));
      end
      disp("grafic",size(y,2), size(xx,2))
      plot(xx,y,'b',best,fb,'r*');
 end     
endfunction

// initializarea populatiei (m vectori avand cate n componente)
// fiecare componenta se genereaza uniform aleator in intervalul [a,b]
function pop = initpop(m,n,a,b)
pop=zeros(m,n);
for i=1:m
   for j=1:n
      pop(i,j)=grand(1,1,'unf',a,b);
   end   
end   
endfunction

// repararea elementelor populatiei; componentele generate in afara
// domeniului [a,b] sunt inlocuite cu valori generate aleator in interiorul
// domeniului

function y=repair(x,a,b)
y=zeros(1,size(x,2));
for j=1:size(x,2)
    if (x(1,j)<a) or (x(1,j)>b)
        y(1,j)=grand(1,1,'unf',a,b);
    else
        y(1,j)=x(1,j);
    end
end    
endfunction

// evaluarea elementelor populatiei
function f = evalpop(functie,x)
for i=1:size(x,1)
    f(1,i)=functie(x(i,:));
end
endfunction

// determinarea celui mai bun element din populatie
function indBest=findBest(f)
indBest=1;
for i=2:size(f,2)
    if f(1,i)<f(1,indBest)
        indBest=i;
    end
end
endfunction

// functie test (sfera)
function y=f1(x)
n=size(x,2);
y=0;
for j=1:n
   y=y+(x(j)-10)^2;
end
endfunction

// functie test (Griewank)
function y=f2(x)
n=size(x,2);
s=0;p=1;
for j=1:n
   s=s+x(j)^2;
   p=p*cos(x(j)/sqrt(j));   
end   
y=s/4000-p+1;
endfunction

// [xBest,fbest]=PSO(m,n,gen,a,b,gama,r1,r2,functie)
[xbest,fbest]=PSO(20,1,50,-50,50,0.1,0.5,0.5,f2)








