%PSO >> function for the PSO ALGORITHM
%
% USAGES:   1.) [fxmin, xmin, Swarm, history] = PSO(psoOptions);
%           2.) [fxmin, xmin, Swarm, history] = PSO;
%           3.) fxmin = PSO(psoOptions);
%           3.) PSO
%           etc.
%
% Arguments     : psoOptions--> A Matlab stucture containing all PSO related options. (see also: get_psoOptions)
% Return Values : [fxmin, xmin, Swarm, history]
%                     |     |       |       |_The history of the algorithm. Depicts how the function value of GBest changes over the run.
%                     |     |       |_The final Swarm. (A matrix containing co-ordinates of all particles)
%                     |     |_The co-ordinates of Best (ever) particle found during the PSO's run.
%                     |__The objective value of Best (^xmin) particle.
%
%  History        :   Author      :   JAG (Jagatpreet Singh)
%                     Created on  :   05022003 (Friday. 2nd May, 2003)
%                     Comments    :   The basic PSO algorithm.
%                     Modified on :   07102003 (Thursday. 10th July, 2003)
%                     Comments    :   It uses psoOptions structure now. More organized.
%
%                     see also: get_psoOptions


% function [fxmin, xmin, Swarm, history, mark_fGBest] = PSO(psoOptions)
%%%%%%%% Globals
global psoFlags;
global psoVars;
global psoSParameters;
global notifications;
%%%%%%% 手動(dòng)改變函數(shù)、參數(shù) %%%%%%%%%
if 1
%     clc;
    clear;
    psoOptions = get_psoOptions;
    psoOptions.Obj.fitnessfunction = 'DeJong';
    psoOptions.Obj.lb = -100;
    psoOptions.Obj.ub = 100;
    psoOptions.SParams.Vmax = 200;    
    psoOptions.Vars.SwarmSize = 50;
    psoOptions.Vars.Dim = 2;
    psoOptions.Vars.Iterations = 1000;
    psoOptions.Flags.Neighbor = 0;
    psoOptions.SParams.w_start = 0.95;
    psoOptions.SParams.w_end = 0.4;
    psoOptions.SParams.w_varyfor = 1;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
GM = 0; % Global minimum (used in the stopping criterion)
ErrGoal = 1e-10; % Desired accuracy

%%%%%%% Initializations
if nargin == 0 % nargin表示輸入?yún)?shù)數(shù)目;
    psoOptions = get_psoOptions;
end

%%%%%%% For Displaying
if psoOptions.Flags.ShowViz
    global vizAxes; %Use the specified axes if using GUI or create a new global if called from command window
    vizAxes = plot(0,0, '.');
    axis([-1000 1000 -1000 1000 -1000 1000]);   %Initially set to a cube of this size
    axis square;
    grid off;
    set(vizAxes,'EraseMode','xor','MarkerSize',15); %Set it to show particles.
    pause(1);
end
%%%%%%%% End Display initialization

%%%%%%%% Initializing variables
success = 0; % Success Flag
iter = 0;   % Iterations' counter
fevals = 0; % Function evaluations' counter,迭代次數(shù)----？？？？？

%%%%%%%% Using params,Determine the value of weight change
%%%%%%%% 速度權(quán)重變化公式 %%%%%%%%
w_start = psoOptions.SParams.w_start;   %Initial inertia weight's value
w_end = psoOptions.SParams.w_end;       %Final inertia weight
w_varyfor = floor( psoOptions.SParams.w_varyfor * psoOptions.Vars.Iterations );
% Weight change step. Defines total number of iterations for which weight is changed.
w_now = w_start;
inertdec = (w_start-w_end)/w_varyfor; %Inertia weight's change per iteration

%%%%%%%% Initialize Swarm and Velocity
SwarmSize = psoOptions.Vars.SwarmSize;
Swarm = rand(SwarmSize, psoOptions.Vars.Dim) * ...
    (psoOptions.Obj.ub-psoOptions.Obj.lb) + psoOptions.Obj.lb;
VStep = rand(SwarmSize, psoOptions.Vars.Dim);

%%%%%%%%% The objective function to optimize
fitness_function = psoOptions.Obj.fitness_function;

%%%%%%%% Find initial function values.
fSwarm = feval(fitness_function, Swarm); %目標(biāo)函數(shù)
fevals = fevals + SwarmSize; %迭代次數(shù)----？？？？？

%%%%%%%%% Initializing the Best positions matrix and
% the corresponding function values
PBest = Swarm; %個(gè)體最優(yōu)位置
fPBest = fSwarm; %個(gè)體最優(yōu)位置的 目標(biāo)函數(shù)最優(yōu)值

% Finding best particle in initial population
[fGBest, g] = min(fSwarm); %全局目標(biāo)函數(shù)最優(yōu)值，全局最優(yōu)位置行數(shù)標(biāo)志g
lastbpf = fGBest; %標(biāo)志：歷史全局目標(biāo)函數(shù)最優(yōu)值
Best = Swarm(g,:); %Used to keep track of the Best particle ever,全局最優(yōu)位置
fBest = fGBest; %標(biāo)志：歷史全局目標(biāo)函數(shù)最優(yōu)值
history = [0, fGBest]; %？？？？？

%%%%%%%%% 定義鄰域拓?fù)浣Y(jié)構(gòu)
if psoOptions.Flags.Neighbor
    % Define social neighborhoods for all the particles
    for i = 1:SwarmSize
        lo = mod(i-psoOptions.SParams.Nhood : i+psoOptions.SParams.Nhood , SwarmSize);
        nhood(i,:) = [lo];
    end
    nhood(find(nhood==0)) = SwarmSize; %Replace zeros with the index of last particle.
end

if psoOptions.Disp.Interval & (rem(iter, psoOptions.Disp.Interval) == 0)
    disp(sprintf('Iterations\t\t\tfGBest\t\t\tfevals'));
end
%%%%%%% 包含高斯分布的分段曲線 %%%%%%%%
w_change = Goss_w(iter,psoOptions.Vars.Iterations);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                  THE  PSO  LOOP                          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while( (success == 0) & (iter <= psoOptions.Vars.Iterations-1) )
    iter = iter+1; %初值為0，從1開(kāi)始
    % Update the value of the inertia weight w
%     if (iter<=w_varyfor) & (iter > 1)
%         w_now = w_now - inertdec; %Change inertia weight
%     end
%     w_now = Goss_w(iter,psoOptions.Vars.Iterations-1);
    w_now = w_change(iter);
%     w_change(iter) = w_now; %繪圖記錄
    %%%%%%%%%%%%%%%%%% The PLAIN PSO %%%%%%%%%
    % Set GBest
    A = repmat(Swarm(g,:), SwarmSize, 1); %A , repeats the matrix X in m rows by n columns.
    B = A; %B will be nBest (best neighbor) matrix
    % use neighborhood model
    % circular neighborhood is used
    if psoOptions.Flags.Neighbor
        for i = 1:SwarmSize
            [fNBest(i), nb(i)] = min( fSwarm( find(nhood(i)) ) );
            B(i, :) = Swarm(nb(i), :);
        end
    end 
    %%%%%%% 定義粒子群平均適應(yīng)度值 %%%%%%%%%%
    f_avg = sum( fSwarm ) / SwarmSize;
    MarkSwarms = fSwarm < f_avg;
    [m,n] = size( find(MarkSwarms) );
    f_avg2 = sum( fSwarm(find(MarkSwarms)) ) / m;
    Beta(iter) = abs( fGBest-f_avg2 );
    KeepSwarms = fSwarm < f_avg2;
    ChangeSwarms = fSwarm >= f_avg2;

    % Generate Random Numbers
    R1 = rand(SwarmSize, psoOptions.Vars.Dim);
    R2 = rand(SwarmSize, psoOptions.Vars.Dim); 
    % Calculate Velocity
    if ~psoOptions.Flags.Neighbor %Normal
        VStep = w_now*VStep + psoOptions.SParams.c1*R1.*(PBest-Swarm) + psoOptions.SParams.c2*R2.*(A-Swarm);
    else %With neighborhood
        R3 = rand(SwarmSize, psoOptions.Vars.Dim); %random nos for neighborhood
        VStep = w_now*VStep + psoOptions.SParams.c1*R1.*(PBest-Swarm) + psoOptions.SParams.c2*R2.*(A-Swarm) ...
            + psoOptions.SParams.c3*R3.*(B-Swarm);
    end
    %%%%%%%%% 按ChangeSwarms變異PBest或VStep %%%%%%%%%%
    [m,n] = size( find(ChangeSwarms) );
    % VStep(find(ChangeSwarms), :) = VStep(find(ChangeSwarms), :) .* (1+0.5*rand(m, psoOptions.Vars.Dim));
    k = 0.5;
    VStep(find(ChangeSwarms), :) = VStep(find(ChangeSwarms), :) * ( 1 + 1/(1+exp( k*Beta(iter) )) );
    
    % Apply Vmax Operator for v > Vmax
    changeRows = VStep > psoOptions.SParams.Vmax;
    VStep(find(changeRows)) = psoOptions.SParams.Vmax;
    % Apply Vmax Operator for v < -Vmax
    changeRows = VStep < -psoOptions.SParams.Vmax;
    VStep(find(changeRows)) = -psoOptions.SParams.Vmax;   

    % ::UPDATE POSITIONS OF PARTICLES::
    Swarm = Swarm + psoOptions.SParams.Chi * VStep;    % Evaluate new Swarm
    
    % Apply ub Operator for Swarm > ub
    changeRows = Swarm > psoOptions.Obj.ub;
    Swarm(find(changeRows)) = psoOptions.Obj.ub;
    % Apply lb Operator for Swarm < lbp
    changeRows = Swarm < psoOptions.Obj.lb;
    Swarm(find(changeRows)) = psoOptions.Obj.lb;    
    
    fSwarm = feval(fitness_function, Swarm);
    fevals = fevals + SwarmSize;

    % Updating the best position for each particle
    changeRows = fSwarm < fPBest;
    fPBest(find(changeRows)) = fSwarm(find(changeRows));
    PBest(find(changeRows), :) = Swarm(find(changeRows), :);    

    lastbpart = PBest(g, :);
    % Updating index g
    [fGBest, g] = min(fPBest); %標(biāo)志：全局目標(biāo)函數(shù)最優(yōu)值
    mark_fGBest(iter) = min(fPBest); %保存全局最優(yōu)進(jìn)化值;
    
    V_mark(iter,:) = VStep(1,:); %第一粒, 速度變化記錄 ---------------------
    P_mark(iter,:) = Swarm(1,:); %第一粒, 粒子位置變化記錄 ---------------------
    BV_mark(iter) = VStep(g,1); %最優(yōu)粒,第一維 速度變化記錄 ---------------------
    BP_mark(iter) = Swarm(g,1); %最優(yōu)粒,第一維 粒子位置變化記錄 ---------------------

    %Update Best. Only if fitness has improved.
    if fGBest < lastbpf        
        [fBest, b] = min(fPBest); %標(biāo)志：歷史全局目標(biāo)函數(shù)最優(yōu)值
        Best = PBest(b,:);  %全局目標(biāo)函數(shù)最優(yōu)位置
    end
    %%%%%%%%% 按ChangeSwarms變異 PBest或VStep %%%%%%%%%%
%     [m,n] = size( find(ChangeSwarms) );
%     k = 0.5;
%     PBest(find(ChangeSwarms), :) = PBest(find(ChangeSwarms), :) * ( 1 + 1/(1+exp( k*Beta(iter) )) );
%     VStep(find(ChangeSwarms), :) = VStep(find(ChangeSwarms), :) * ( 1 + 1/(1+exp( k*Beta(iter) )) );

    %%OUTPUT%%
    if psoOptions.Save.Interval & (rem(iter, psoOptions.Save.Interval) == 0)
        history( (size(history,1)+1), : ) = [iter, fBest];
    end

    if psoOptions.Disp.Interval & (rem(iter, psoOptions.Disp.Interval) == 0)
        disp(sprintf('%4d\t\t\t%10f\t\t\t%5d', iter, fGBest, fevals));
    end

    if psoOptions.Flags.ShowViz
        [fworst, worst] = max(fGBest);
        DrawSwarm(Swarm, SwarmSize, iter, psoOptions.Vars.Dim, Swarm(g,:), vizAxes);
    end

    %%TERMINATION%%
    if abs(fGBest-psoOptions.Obj.GM) <= psoOptions.Vars.ErrGoal     %GBest
        success = 0;
    elseif abs(fBest-psoOptions.Obj.GM)<=psoOptions.Vars.ErrGoal    %Best
        success = 0;
    else
        lastbpf = fGBest; %To be used to find Best
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                  END  OF PSO  LOOP                       %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k = size(mark_fGBest);
semilogy( 1:k(2), mark_fGBest,'r:' );
figure(2);
subplot(4,1,1); semilogy( 1:k(2), mark_fGBest,'r:' ); grid on; title('The First P, 1x1'); %plot( 1:k(2), mark_fGBest,'r:');
subplot(4,1,2); plot( 1:k(2),w_change,'b:'); grid on;
subplot(4,1,3); plot( 1:k(2),V_mark(:,1),'b:'); grid on; ylabel('V');
subplot(4,1,4); plot( 1:k(2),P_mark(:,1),'b:'); grid on; ylabel('P');
figure(3);
subplot(4,1,1); semilogy( 1:k(2), mark_fGBest,'r:' ); grid on; title('The First P, 1x2'); %plot( 1:k(2), mark_fGBest,'r:');
subplot(4,1,2); plot( 1:k(2),w_change,'b:'); grid on;
subplot(4,1,3); plot( 1:k(2),V_mark(:,2),'b:'); grid on; ylabel('V');
subplot(4,1,4); plot( 1:k(2),P_mark(:,2),'b:'); grid on; ylabel('P');
figure(4);
subplot(4,1,1); semilogy( 1:k(2), mark_fGBest,'r:' ); grid on; title('The First Particle 1x3'); %plot( 1:k(2), mark_fGBest,'r:');
subplot(4,1,2); plot( 1:k(2),w_change,'b:'); grid on;
subplot(4,1,3); plot( 1:k(2),V_mark(:,3),'b:'); grid on; ylabel('V');
subplot(4,1,4); plot( 1:k(2),P_mark(:,3),'b:'); grid on; ylabel('P');

figure(5);
subplot(5,1,1); semilogy( 1:k(2), mark_fGBest,'r:' ); grid on; title('The Best particle'); %plot( 1:k(2), mark_fGBest,'r:');
subplot(5,1,2); plot( 1:k(2),w_change,'b:'); grid on;
subplot(5,1,3); plot( 1:k(2),BV_mark,'b:'); grid on; ylabel('BV');
subplot(5,1,4); plot( 1:k(2),BP_mark,'b:'); grid on; ylabel('BP');
subplot(5,1,5); plot( 1:k(2),Beta,'b:'); grid on; ylabel('Beta');
figure(6);
plot(w_change);

[fxmin, b] = min(fPBest)
xmin = PBest(b, :)
history = history(:,1);
%Comment below line to Return Swarm. Uncomment to return previous best positions.
% Swarm = PBest; %Return PBest