% Particle Swarm optimalisation
% BSDLicence
% Rick van der Zwet - 0433373 - <hvdzwet@liacs.nl>
% Modeled after http://en.wikipedia.org/wiki/Particle_swarm_optimization

% Parameters
iterations = 1;
parameters = 3;
local_swarm_size = 2;
local_swarms = 5;
wander = 0.5;

% 'Influence' of the envirionment with regards to solutions
% Trust the group global solution to be feasible
c_social  = 0.4;
% Trust the neighbor solution to be feasible
c_cognitive = 0.4;
% Trust the own best solution to be feasible
c_ego = 0.2;

% Initiate all particles
flock_p = rand(parameters,local_swarm_size,local_swarms) .* (2 * pi);
flock_v = zeros(size(flock_p));


% Global best placeholder
g_best = zeros(parameters,1);
g_fitness = 1;
% at (:,x) lives the neighbor best of local_swarm 'x'
n_best = zeros(parameters,local_swarms);
n_fitness = ones(local_swarms);
% at (:,p,x) leves the local best of particle 'p' in local_swarm 'x'
l_best = zeros(parameters,local_swarm_size,local_swarms);
l_fitness = ones(local_swarm_size, local_swarms);


% Code not optimised for performance, but for readablility
for i = 1:iterations
    for s = 1:local_swarms
        fitness = SHGa(flock_p(:,:,s));
        % See if we got any better local optimum
        for p = 1:local_swarm_size
            if fitness(p) < l_fitness(p,s)
                l_fitness(p,s) = fitness(p);
                l_best(:,p,s) = flock_p(:,p,s);
            end
        end

        % See if we got any better neighbor optimum
        for p = 1:local_swarm_size
            if l_fitness(p,s) < n_fitness(s)
                n_fitness(s) = l_fitness(p,s);
                n_best(:,s) = l_best(:,p,s);
            end
        end
    end

    % See wether we have a new global optimum
    for s = 1:local_swarms
        if n_fitness(s) < g_fitness
          g_fitness = n_fitness(s);
          g_best = n_best(s);
        end
    end

    % Update particles to new value
    r_cognitive = rand();
    r_social = rand();
    r_ego = rand();
    for s = 1:local_swarms
        for p = 1:local_swarm_size
            flock_v(:,p,s) = flock_v(:,p,s) * wander;
            flock_v(:,p,s) =+ g_best * (c_cognitive * r_cognitive);
            flock_v(:,p,s) =+ n_best(s) * (c_social * r_social);
            flock_v(:,p,s) =+ l_best(p,s) .* (c_ego *r_ego);
            flock_p(:,p,s) =+ flock_v(:,p,s);
        end
    end
end
i = 1:10;
y = i;
plot(i,y);
print -deps pso-fitness.eps;
%title(sprintf('Particle Swarm Optimalisation on Laser-Pulse shaping problem - repetitions %i',repetitions));
%ylabel('fitness');
%xlabel('iterations');
%grid on;
%legend(sprintf('Length %i',length));
%print("sa-fitness.eps","-depsc2");

