clear all
clc
% Algorytm PSO
 
%Definicje
 
dg      = -5.12;    % dolna graica przedizału
gg      =  5.12;    % górna granica przedizału
krok    =  0.01; 
 
osob    =  1000;      % ilość osobników w roju
 
ile     =  30;      % ilość wymiarów
 
% Poziomy ufności
 
c1      = 0.3;      % zaufanie do własnego kierunku
c2      = 0.4;      % zaufanie swojej najlepszej pozycji
c3      = 0.7;      % zaufanie najlepszej pozycji sąsidów
 
% Prędkość początkowa
V(1,:)  = [0,0];
 
max     = 100;     % ilość iteracji
 
%% Generacja osobników w roju
X=dg:krok:gg;
rozmiar=length(X);
X=X';
for i=1:osob
 
    a=randi(rozmiar,1);
    os(i,1)=X(a,:);
    a=randi(rozmiar,1);
    os(i,2)=X(a,:);
    
end
 
for k=1:max
%% Wylicznie wartości funkcji 
 
for i=1:osob
    a = os(i,1)^2;
    b = os(i,2)^2;
    c = cos(2*pi*os(i,1));
    d = cos(2*pi*os(i,2));
    os(i,3)= ((a-(c*ile))+(b-(d*ile)));
    
end
 
plot3(os(:,1),os(:,2),os(:,3),'ro')
title('Położenie osobników')
%plot(os(:,1),os(:,2),'ro') % kontrola
 
%% Wybór najlepszego osobnika
nr=0;
M=os(:,3)';
b=min(M);
for i=1:osob
        
        if os(i,3) == b
            nr = i;
        end
     
end
%% Przkazanie danych o najlepszym rozwiazaniu danej iteracji
 
X(k,:)=os(nr,1);
Y(k,:)=os(nr,2);
Z(k,:)=os(nr,3);
 
%% Zapis Najlepszego ogólnego położenia
if k == 1
    opti=Z(k,:);
    px=X(k,:);
    py=Y(k,:);
end
 
if k > 1
    if Z(k,:) < opti
        opti=Z(k,:);
        px=X(k,:);
        py=Y(k,:);
    else
        opti=opti;
        px=px;
        py=py;
    end
end
 
   
%% Nadanie prędkości
for i=1:osob
    r1  = rand(1,1);
    r2  = rand(1,1);
    r3  = rand(1,1);
    ax   = c1*r1*V(i,1);
    ay   = c1*r1*V(i,2);
    bx   = c2*r2*(X(k,:)-os(i,1));
    by   = c2*r2*(Y(k,:)-os(i,2));
    c   =  c3*r3*(px-py);
    u=i+1;
    V(u,1)=ax+bx+c;
    V(u,2)=ay+by+c;
end
%% Aktualizacja położenia 
 
for i=1:osob
 
    os(i,1) =   os(i,1)+V(i+1,1);    
    os(i,2) =   os(i,2)+V(i+1,2) ;   
    drawnow
 
end
 
OP(k,:)=opti;
czas(k,:)=k;
pp(k,:)=px;
ppp(k,:)=py;
end
 
rysuj
disp('Znalazłem min o wartości')
disp(opti)
disp('w')
disp(px)
disp(py)