%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 :   0710003 (Thursday. 10th July, 2003)
%                     Comments    :   It uses psoOptions structure now. More organized.
%  
%                     see also: get_psoOptions
function [fxmin, xmin, Swarm, history] = pso_proposed(psoOptions)

%Globals
% global psoFlags;
% global psoVars;
% global psoSParameters;
% global notifications;


% upbnd = 600; % Upper bound for init. of the swarm
% lwbnd = 300; % Lower bound for init. of the swarm
% GM = 0; % Global minimum (used in the stopping criterion)
% ErrGoal = 1e-10; % Desired accuracy
% 

%Initializations
if nargin == 0
    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

% Using params---
% Determine the value of weight change

% 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;
lb = psoOptions.Obj.lb;%repmat(psoOptions.Obj.lb,SwarmSize,1);
ub = psoOptions.Obj.ub;%repmat(psoOptions.Obj.ub,SwarmSize,1);
Swarm = rand(SwarmSize, psoOptions.Vars.Dim).*(ub-lb) + lb;
VStep = rand(SwarmSize, psoOptions.Vars.Dim);

% AIWF
w_max = psoOptions.SParams.w_start;   %Initial inertia weight's value
w_min = psoOptions.SParams.w_end;       %Final inertia weight
w = rand(SwarmSize,1)*(w_max-w_min)+w_min;

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

%Find initial function values.
fSwarm = feval(f2eval, Swarm);
fevals = fevals + SwarmSize;

% Initializing the Best positions matrix and
% the corresponding function values
PBest = Swarm;
fPBest = fSwarm;
aPBest = fSwarm; %it will contain the best result obtained only after the jump

% Finding best particle in initial population
[fGBest, g] = min(fSwarm);
lastbpf = fGBest;
Best = Swarm(g,:); %Used to keep track of the Best particle ever
fBest = fGBest;
history = [0, fGBest];

% if psoOptions.Flags.Neighbor
%     % Define social neighborhoods for all the particles
%     for i = 1:SwarmSize
%         lo = mod(i-psoOptions.SParam.Nhood:i+psoOptions.SParam.Nhood, SwarmSize);
%         nhood(i,:) = [lo];
%     end
%     nhood(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\tfGBest\t\t\tfevals'));
end

% R1 = zeros(SwarmSize, psoOptions.Vars.Dim);
% R2 = zeros(SwarmSize, psoOptions.Vars.Dim);

n_stagnation = zeros(SwarmSize,1);
limit_stagnation = 20;
reinit_eta = 1.6;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                  THE  PSO  LOOP                          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while( (success == 0) && (iter <= psoOptions.Vars.Iterations) )
    iter = iter+1;
    
    % Update the value of the inertia weight w
%     if (iter<=w_varyfor) & (iter > 1)
%         w_now = w_now - inertdec; %Change inertia weight
%     end
    % Update w as AIWF
    if iter > 1
        f_avg = mean(fSwarm);
        f_min = min(fSwarm);
        elems_to_set = fSwarm <= f_avg;
        elems_to_max = fSwarm > f_avg;
        w(elems_to_set) = w_min + ((w_max-w_min)*(fSwarm(elems_to_set)-f_min))./(f_avg-f_min);
        w(elems_to_max) = w_max;
    end
    
    
    %%%%%%%%%%%%%%%%%
    % The PLAIN PSO %
    
    % Set GBest
    A = repmat(Swarm(g,:), SwarmSize, 1); %A = GBest. repmat(X, m, n) repeats the matrix X in m rows by n columns.
%     B = A; %B wil 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( nhood(i) ));
%             B(i, :) = Swarm(nb(i), :);
%         end
%     end
        
%     % Generate Random Numbers according to MPSO
%     pi = sign(PBest - Swarm);
%     ni = sign(A - Swarm);
%     gauss_update = sum(pi==ni,2)==psoOptions.Vars.Dim; %keep only the particle which signs are equals
% %     R1(gauss_update,:) = randn(sum(gauss_update(:,1)), psoOptions.Vars.Dim); %gaussian
% %     R2(gauss_update,:) = randn(sum(gauss_update(:,1)), psoOptions.Vars.Dim);
%     R1(gauss_update,:) = cauchyrnd(0,1,[sum(gauss_update(:,1)), psoOptions.Vars.Dim]); %cauchy
%     R2(gauss_update,:) = cauchyrnd(0,1,[sum(gauss_update(:,1)), psoOptions.Vars.Dim]);
%     R1(not(gauss_update),:) = rand(sum(not(gauss_update(:,1))), psoOptions.Vars.Dim);
%     R2(not(gauss_update),:) = rand(sum(not(gauss_update(:,1))), psoOptions.Vars.Dim);
    
    %Original
    R1 = rand(SwarmSize, psoOptions.Vars.Dim);
    R2 = rand(SwarmSize, psoOptions.Vars.Dim);
    
    % Calculate Velocity
%     if ~psoOptions.Flags.Neighbor %Normal
    for row = 1:SwarmSize %it is a multiply row per row, I don't know how to do it
        VStep(row,:) = w(row).*VStep(row,:);
    end
    VStep = 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.*VStep + psoOptions.SParams.c1*R1.*(PBest-Swarm) + psoOptionsSParams.c2*R2.*(A-Swarm) + psoOptionsSParams.c3*R3.*(B-Swarm);
%     end
    
    % Apply Vmax Operator for v > Vmax
    changeRows = VStep > psoOptions.SParams.Vmax;
    VStep(changeRows) = psoOptions.SParams.Vmax;
    % Apply Vmax Operator for v < -Vmax
    changeRows = VStep < -psoOptions.SParams.Vmax;
    VStep(changeRows) = -psoOptions.SParams.Vmax;
    
    % ::UPDATE POSITIONS OF PARTICLES::
    %with reinit
    reinit = n_stagnation == limit_stagnation;
    Swarm(reinit,:) = PBest(reinit,:).*(1+reinit_eta*randn(sum(reinit),psoOptions.Vars.Dim));
%     Swarm(reinit,:) = rand(sum(reinit),psoOptions.Vars.Dim).*(ub-lb)+lb;
    n_stagnation(reinit)=0; %reset counter when reinitialization is made
    
    Swarm = Swarm + psoOptions.SParams.Chi * VStep;    % Evaluate new Swarm
%     elems = Swarm < lb;
    Swarm(Swarm < lb) = lb - (Swarm(Swarm < lb) - lb)/2; %rebounds
%     elems = Swarm > ub;
    Swarm(Swarm > ub) = ub - (Swarm(Swarm > ub) - ub)/2;

    
    fSwarm = feval(f2eval, Swarm);
    fevals = fevals + SwarmSize;
    
    aPBest(reinit) = fSwarm(reinit); %re-initialize gli apBest
    
    % Updating the best position for each particle
    %first, only the ones after the jump
    changeRows = fSwarm < aPBest;
    n_stagnation(changeRows) = 0;  %with reinit
    n_stagnation(not(changeRows)) = n_stagnation(not(changeRows)) + 1;
    aPBest(changeRows) = fSwarm(changeRows);
    %then, historically
    changeRows = fSwarm < fPBest;
    fPBest(changeRows) = fSwarm(changeRows);
    PBest(changeRows, :) = Swarm(changeRows, :);
    
%     lastbpart = PBest(g, :);
    % Updating index g
    [fGBest, g] = min(fPBest);

    %Update Best. Only if fitness has improved.
    if fGBest < lastbpf
        [fBest, b] = min(fPBest);
        Best = PBest(b,:);
    end
    
    %%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%.5g\t\t\t%5d', iter, fGBest, fevals));
        %Best
    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 = 1;
    elseif abs(fBest-psoOptions.Obj.GM)<=psoOptions.Vars.ErrGoal    %Best
        success = 1
    else
        lastbpf = fGBest; %To be used to find Best
    end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                  END  OF PSO  LOOP                       %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



[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
