% This program finds the maximum of the function using
% particle swarm optimization.
clear; clc
% fun_num=input('Select function: 1-Rastrigin, 2-Rosenbrock, 3-Ackley, 4-Griewank, 5-double sinus, 6-exp_sin: ')
% [x y yy]=select_function(fun_num);
x=1:0.01:10;
y1=sin(x);
y2=log(x)/8;
y=y1+y2;
plot(x,y)
grid on

x_max=max(x);
x_min=min(x);
% range of the indipendent variable x
% x_max=2;
% x=1:0.01:10;
% y=sin(x)+log(x)/8;
% plot(x,yy)
hold on

% value of parameters
omega=0.9; c1=1; c2=1; k=0.5;
v_max=(k*(x_max-x_min))/2;
% nop=input('Enter the number of particles: ')
nop=10;
% noi=input('Enter the number of iterations: ')
noi=200;

% initialization of particles
part_x_init=round(100*((x_max-x_min)*rand(1,nop)+x_min))/100; % 10 random values between -2 and 2
% fitness
fitness=inline('sin(x)+log(x)/8');
% fitness=y;
part_y_init=fitness(part_x_init);
plot(part_x_init,part_y_init,'rs')
% init velocities
v_init=(v_max-(-v_max))*rand(1,nop)-v_max;
% find best values for each particle and global best
part_x_best=part_x_init;
part_y_best=part_y_init;
[u v]=max(part_y_best);
g_best_x=part_x_best(v);
g_best_y=u;
v_prev=v_init;
part_x_prev=part_x_init;
for i=1:noi
    r1=rand; r2=rand;
    v_next=omega*v_prev+c1*r1*(part_x_best-part_x_prev)+c2*r2*(g_best_x-part_x_prev);
    
    for jj=1:nop
        if v_next(jj)>v_max
            v_next(jj)=v_max;
        end
        if v_next(jj)<-v_max
            v_next(jj)=-v_max;
        end
    end
    part_x_next=part_x_prev+v_next;
    for ii=1:nop
        if (part_x_next(ii)>x_max)
            part_x_next(ii)=(x_max-x_min)*rand+x_min;
        end
        if (part_x_next(ii)<x_min)
            part_x_next(ii)=(x_max-x_min)*rand+x_min;
        end
    end
    part_y_next=fitness(part_x_next);
    clf;
    plot(x,y)
    hold on
    plot(part_x_next,part_y_next,'rs')
    plot(g_best_x,g_best_y,'gs','MarkerFaceColor','g')
    pause(0.1)
    % update
    w=0.9-i*(0.5/noi); % megkerdezni hogy ez honnan jon?
    v_prev=v_next;
    part_x_prev=part_x_next;
    part_y_prev=fitness(part_x_prev);
    [u v]=max(part_y_prev);
    g_best_y_current=u;
    g_best_x_current=part_x_prev(v);
    if g_best_y_current>g_best_y
        g_best_y=g_best_y_current;
        g_best_x=part_x_prev(v);
    end
    for j=1:nop
        if part_y_prev(j)>part_y_best(j)
            part_y_best(j)=part_y_prev(j);
            part_x_best(j)=part_x_prev(j);
        end
    end
end