% ch13_particle_swarm.m
% Companion code for Chapter 13: Optimization and Data Fitting.
%
% Bounded minimization by particle swarm optimization (PSO).
% Change the objective, bounds, and search settings below. No derivatives
% are required. The routine accepts any number of variables; the optional
% contour plot is for two. Each objective call receives a column vector p.
% Repeat with different seeds and search budgets to assess reliability.
% The best point found is not proof of a global minimum.

clear; close all; clc;

%% Problem and settings
f = @(p) sin(3*p(1))*sin(3*p(2)) + 0.1*(p(1)^2 + p(2)^2);
lower = [-3; -3];          % Lower bound for each variable
upper = [ 3;  3];          % Upper bound for each variable

swarm_size = 25;           % Number of particles
max_iter = 150;            % Number of swarm updates
inertia = 0.72;            % Retention of previous velocity
c_personal = 1.4;          % Attraction to each particle's best position
c_global = 1.4;            % Attraction to the swarm's best position
vmax = [0.5; 0.5];         % Maximum absolute velocity for each variable
seed = 2;                 % Fixed seed for reproducible runs
make_plot = true;

rng(seed, 'twister');
% Alternative: comment out the line above and uncomment the line below
% to initialize from the current time for a different run each time.
% rng('shuffle');

%% Minimize
[pbest, fbest, info] = particle_swarm(f, lower, upper, swarm_size, ...
    max_iter, inertia, c_personal, c_global, vmax);

%% Results
fprintf('\nParticle swarm optimization\n');
for j = 1:numel(pbest)
    fprintf('Best p(%d) = %.12g\n', j, pbest(j));
end
fprintf('Best objective value: %.12g\n', fbest);
fprintf('Swarm updates:        %d\n', info.iterations);
fprintf('Function calls:       %d\n', info.evaluations);
fprintf('Status:               %s\n', info.status);
fprintf('Repeat with other seeds; global optimality is not guaranteed.\n');

%% Optional contour plot
if make_plot && numel(pbest) == 2
    [X, Y] = meshgrid(linspace(lower(1), upper(1), 200), ...
                     linspace(lower(2), upper(2), 200));
    Z = arrayfun(@(x,y) f([x; y]), X, Y);
    figure('Color', 'w');
    [~, hc] = contour(X, Y, Z, 25, 'LineColor', [0.55 0.55 0.55]);
    hold on;
    hb = plot(pbest(1), pbest(2), 'k+', 'MarkerSize', 11, 'LineWidth', 1.5);
    xlabel('p_1', 'FontSize', 14);
    ylabel('p_2', 'FontSize', 14);
    title('Particle swarm optimization', 'FontSize', 14, 'FontWeight', 'normal');
    legend([hc, hb], {'f(p_1,p_2)', 'Best point found'}, 'Location', 'best');
    set(gca, 'FontSize', 14);
    axis equal;
    xlim([lower(1) upper(1)]); ylim([lower(2) upper(2)]);
    grid on; box on;
    % Plot evaluations are excluded from the reported function-call count.
end

%% Reusable numerical routine
function [pbest, fbest, info] = particle_swarm(f, lower, upper, N, ...
        max_iter, inertia, c_personal, c_global, vmax)
% Columns of X and V contain particle positions and velocities.
% All velocities use the same saved global best in each swarm update.
% Independent random factors are drawn for each particle and coordinate.
% Overshoots are reflected at the bounds; reflected velocities are halved.
% vmax must not exceed the interval width, so one reflection is sufficient.
% Copy this routine and its helper to particle_swarm.m for reuse.

    validateattributes(lower, {'double'}, {'real','vector','finite','nonempty'});
    validateattributes(upper, {'double'}, {'real','vector','finite','nonempty'});
    validateattributes(vmax, {'double'}, {'real','vector','finite','positive'});
    lower = lower(:); upper = upper(:); vmax = vmax(:);
    d = numel(lower);
    if numel(upper) ~= d || numel(vmax) ~= d
        error('Bounds and velocity limits must have the same length.');
    end
    width = upper - lower;
    if any(~isfinite(width)) || any(width <= 0) || any(vmax > width)
        error('Require finite upper > lower and vmax <= upper - lower.');
    end
    validateattributes(N, {'double'}, {'real','scalar','finite','integer','>=',2});
    validateattributes(max_iter, {'double'}, ...
        {'real','scalar','finite','integer','positive'});
    validateattributes(inertia, {'double'}, {'real','scalar','finite','>=',0,'<',1});
    validateattributes(c_personal, {'double'}, {'real','scalar','finite','nonnegative'});
    validateattributes(c_global, {'double'}, {'real','scalar','finite','nonnegative'});

    L = repmat(lower, 1, N); U = repmat(upper, 1, N);
    Vmax = repmat(vmax, 1, N);
    X = L + (U - L).*rand(d, N);
    V = Vmax.*(2*rand(d, N) - 1);
    personal = X;
    personal_f = zeros(1, N);
    for i = 1:N
        personal_f(i) = checked_value(f, X(:,i));
    end
    [fbest, idx] = min(personal_f);
    pbest = personal(:,idx);

    for k = 1:max_iter
        V = inertia*V + c_personal*rand(d,N).*(personal - X) ...
            + c_global*rand(d,N).*(repmat(pbest,1,N) - X);
        V = max(min(V, Vmax), -Vmax);
        X = X + V;

        below = X < L; above = X > U;
        X(below) = 2*L(below) - X(below);
        X(above) = 2*U(above) - X(above);
        V(below | above) = -0.5*V(below | above);
        X = max(min(X, U), L);  % Remove any roundoff-sized overshoot.

        for i = 1:N
            value = checked_value(f, X(:,i));
            if value < personal_f(i)
                personal(:,i) = X(:,i);
                personal_f(i) = value;
            end
        end
        [fbest, idx] = min(personal_f);
        pbest = personal(:,idx);
    end

    info = struct('iterations', max_iter, 'evaluations', N*(max_iter + 1), ...
        'status', 'Search budget completed.');
end

function value = checked_value(f, p)
    value = f(p);
    if ~isnumeric(value) || ~isscalar(value) || ~isreal(value) || ~isfinite(value)
        error('The objective must return a finite real scalar.');
    end
end