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

% Dimention settings
parameters = 10;

% If global optimum does not change this many steps bail out 
iteration_break = 5;
max_iterations = 1000;
max_time = 10 * 60; % in sec

% Flock properties 
local_swarm_size = 50;
local_swarms = 10;

%% Particle properties
% Speed of walking around to a certain direction
wander = 0.4;

% '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;

% Variables used for plotting
fitness_history = [];
fitness_iterations = [];

% 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 = ones(parameters,1) .* 9;
g_fitness = 0;
% at (:,x) lives the neighbor best of local_swarm 'x'
n_best = ones(parameters,local_swarms) .* 9;
n_fitness = zeros(parameters,local_swarms);
% at (:,p,x) leves the local best of particle 'p' in local_swarm 'x'
l_best = ones(parameters,local_swarm_size,local_swarms) .* 9;
l_fitness = zeros(local_swarm_size, local_swarms);

idle_counter = 0;
tic();

% Code not optimised for performance, but for readablility
for i = 1:max_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

    idle_counter = idle_counter + 1;

    % 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);
          idle_counter = 0;
        end
    end

    % Stop conditions
    if idle_counter == iteration_break 
        fprintf('Caught by idle_counter\n');
        break;
    end
    if toc > max_time
        fprintf('Caught by max_time used \n');
        break;
    end
    

    fprintf('%04i : %.15f\n', i, g_fitness);
    fitness_iterations = [fitness_iterations, i];
    fitness_history = [fitness_history, g_fitness];
    

    % 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 + ...
              (g_best - flock_p(:,p,s)) * (c_cognitive * r_cognitive) + ...
              (n_best(:,s) - flock_p(:,p,s)) * (c_social * r_social) + ...
              (l_best(:,p,s) - flock_p(:,p,s)) * (c_ego * r_ego);
            flock_p(:,p,s) = flock_p(:,p,s) + flock_v(:,p,s);
        end
    end
end

% Dispay hack
g_fitness
g_best

plot(fitness_iterations,fitness_history);
title(sprintf('Particle Swarm Optimalisation on Laser-Pulse shaping problem'));
ylabel('fitness');
xlabel('iterations');
grid on;
legend(sprintf('Parameters %i',parameters));
print(sprintf('pso-fitness-%.10f.eps', max(fitness_history)),'-depsc2');