% This program implements a clean version of Particle Swarm Optimization (PSO)
% It is distributed free of charge to increase awareness and encourage experimentation 
% with the algorithm.
% 
% Please email questions, comments, and significant results to: Robert.C.Green@gmail.com
% Thanks!
% 
% (c) 2011 Robert C. Green II
% 
% ALL RIGHTS RESERVED WORLDWIDE
% 
% THIS PROGRAM IS FREEWARE.  IT MAY BE COPIED AND DISTRIBUTED WITHOUT LIMITATION AS 
% LONG AS LONG AS NO FEE OR COMPENSATION IS CHARGED, INCLUDING "TIE-IN" OR "BUNDLING" 
% FEES CHARGED FOR OTHER PRODUCTS

function  PSO(Np, Nd, Nt, xMin, xMax, vMin, vMax, R)
% This function performs Particle Swarm Optimization using various parameters:
% Np 	- Number of Particles
% Nd 	- Number of Dimensions
% Nt 	- Number of Time Steps
% xMin 	- Scalar minimum for particle position
% xMax 	- Scalar maximum for particle position
% vMin 	- Scalar minimum for particle velocity
% vMin 	- Scalar maximum for particle velocity
% R	 	- Optional parameter to specify initial positions

    w  = 0.25;	% Intertial Constant
    C1 = 2.05;	% Constant 1
    C2 = 2.05;	% Constant 2
    
    phi = C1 + C2;	
    chi = 2.0/abs(2.0-phi-sqrt(phi^2-4*phi)); % Constriction Factor
  
	% Init personal best values
    pBestValue 	  = ones(Np) .* -Inf;
    pBestPosition = zeros(Np, Nd);

	% Init global best values
    gBestValue    = -Inf;
    gBestPosition = zeros(Nd);

	% Record of best Fitness for plotting
    bestFitnessHistory = [];
    
	% Initialize position of each particle
	if(nargin < 8) 
		R = zeros(Np, Nd); % Position
		for p=1:Np % For each Particle
			for i =1:Nd % For each dimension
				R(p,i) =  xMin + (xMax-xMin) * rand;
				if rand < 0.5
					R(p,i) = -R(p,i);
				end
			end
		end
	end
    
	% Initialize velocity of each particle
	V = zeros(Np, Nd); % Velocity
    for p=1:Np % For each Particle
       for i =1:Nd % For each dimension
           V(p,i) =  vMin + (vMax-vMin) * rand;
           if rand < 0.5
               V(p,i) = -V(p,i);
           end
       end
    end
    
    % Evaluate Fitness (Optional outside of Loop)
	% Modify this section to use a different fitness function
	% The curretn function is sum(x^2)
	M = zeros(Np);     % Fitness
    for p=1:Np
        M(p) = 0;
        for i=1:Nd
            M(p) = M(p) + R(p,i)^2; 
        end
    end

    
    for j=1:Nt % For each time step
        
        % Update Position
         for p=1:Np
            for i=1:Nd
                R(p,i) = R(p,i) + V(p,i);
				
				% Correct any errors
                if R(p,i) > xMax
                    R(p,i) = xMax;
                elseif  R(p,i) < xMin
                    R(p,i) = xMin;
                end
                
                if rand < 0.5
                    R(p,i) = -R(p,i);
                end
            end
        end
        
        % Evaluate Fitness
        for p=1:Np
            M(p) = 0;
            for i=1:Nd
                M(p) = M(p) + (-R(p,i)^2);
            end
            
            % Check if it is a personal best
            % If it is, record the value and the position
            if M(p) > pBestValue(p)                
                pBestValue(p) = M(p);
                for i=1:Nd
                    pBestPosition(p,i) = R(p,i);
                end 
            end
            
            % Check if it is a global best
            % If it is, record the value and the position
            if M(p) > gBestValue                
                gBestValue = M(p);
                for i=1:Nd
                    gBestPosition(p,i) = R(p,i);
                end 
            end            
        end
        bestFitnessHistory(j) = gBestValue;
        
        % Calculate Velocity
        for p=1:Np
            for i=1:Nd
				% Velocity update using constriction factor chi
				V(p,i)  = chi*(V(p,i) + rand * C1 * (pBestPosition(p,i)-R(p,i)) + rand * C2 * (gBestPosition(i) - R(p,i)));
			   
				% Classic Velocity update formulat
                %V(p,i)  = V(p,i) + w*(rand * C1 * (pBestPosition(p,i)-R(p,i)) + rand * C2 * (gBestPosition(i) - R(p,i)));
                if V(p,i) > vMax
                    V(p,i) = vMax;
                elseif  V(p,i) < vMin
                    V(p,i) = vMin;
                end
            end
        end
		
		% Output global best value at current timestep
        str = sprintf('Iteration: %d Best Value: %f', j, gBestValue);
        disp(str)
    end
	
	% Plot history of best fitness
    plot(bestFitnessHistory);
end