//Particle Swarm Example
//www.timestocome.com

//This program takes a group of particles and moves them about looking 
// to find the maximum value of a 'Mexican Hat Function'  ( sin ( sqrt(x^2+y^2)) ) /  sqrt( x^2 + y^2 + eplison ) )
// between +/- 6.0

import java.util.*;



public class ParticleSwarm
{
		
	static double groupBestHeight = 0.0;	// max height (z) found by any particle
	static double groupBestX;				// position of particle with best height
	static double groupBestY;
	static double groupBestParticle;		// particle number with best height
	
	static int maximum = 6;					// this sets up a -maximum->+maximum square grid area to check 
	static int maximumNumberOfLoops = 100;
	static int maxNumberOfParticles = 6;
	
	static Particle particles[] = new Particle[maxNumberOfParticles];
	
	

	public static void main ( String[] args )
	{
			
		//initialize particles
		for ( int i=0; i< maxNumberOfParticles; i++){
						
			particles[i] = new Particle( maximum );
		}//end for numberOfParticles
				
				
		//loop till we reach maximum number of loops
		//i.e. keep particles swarming for a given number of steps
		for ( int i=0; i< maximumNumberOfLoops; i++ ){
			
			
			//move each particle
			for ( int j=0; j<maxNumberOfParticles; j++ ){
				particles[j].moveParticle();
			}
				
				
			//check each particle and store the best of the group in group best
			for ( int j=0; j<maxNumberOfParticles; j++){
			
				if ( particles[j].best > groupBestHeight ){
					groupBestHeight = particles[j].best;
					groupBestParticle = j;
					groupBestX = particles[j].x;
					groupBestY = particles[j].y;
				}
			}
			
			
			//update each particles velocity
			for ( int j=0; j<maxNumberOfParticles; j++) {
				particles[j].updateParticleVelocity( groupBestX, groupBestY );
			
			}
			
			
			System.out.println ( "ParticleNo: " + groupBestParticle + "  Best so far: " + groupBestHeight + "  x: " + groupBestX + "  y: " + groupBestY );
		}
	
	}//end main
	
	
	


}//end class ParticleSwarm



//**********************************************************************************

class Particle
{
	double x, y, oldX, oldY;
	double velocityX, velocityY;
	double best, previousBest;
	double bestX, bestY;
	double current;
	double dt = .1;				//time step
	double constantParticle = 0.5;
	double constantGroup = 0.5;
	double epsilon = 0.0001;	//keep particle from blowing  up at ( 0, 0 ) 
	double maximum;
	//double choke = .5;				//if velocity gets too high choke it back
	
	static Random random = new Random ( System.currentTimeMillis() );
	
	//set things up
	Particle( int max )
	{		
		
		maximum = max;
		int negative;
		x = random.nextDouble() * maximum;
		y = random.nextDouble() * maximum;
		velocityX = random.nextDouble() * maximum;
		velocityY = random.nextDouble() * maximum;
			
		//toss in a few negative numbers so all the particles are not starting
		//out piled up in one quadrant of the area	
		negative = random.nextInt ( 20 );
		if ( negative%2 == 0 ){ x *= -1; }
		
		negative = random.nextInt ( 20 );
		if ( negative%2 == 0 ){ y *= -1; }
			
		negative = random.nextInt ( 20 );
		if ( negative%2 == 0 ){ velocityX *= -1; }
			
		negative = random.nextInt ( 20 );
		if ( negative%2 == 0 ){ velocityY *= -1; }
	
		//we haven't moved yet
		oldX = x;
		oldY = y;
		
		best = 0.0;
		current = 0.0;
		
		//System.out.println ( "Starting locations: x " + x + ";  y " + y + ";  vx " + vx + ";  vy " + vy ); 
	}
	
	

	void moveParticle()
	{
		double decreaseTimeStep = .99;	//keeps velocity in check
	
		oldX = x;
		oldY = y;
		previousBest = best;
		
		//move particle
		//decrease time step to keep velocity from growing too large
		x += velocityX * (dt * decreaseTimeStep);
		y += velocityY * (dt * decreaseTimeStep);
		
		
		//are we still inside our grid?  if not bounce or wrap or something back onto our search space
		if ( ( x < (maximum*(-1)) ) || ( x > maximum ) ){
			velocityX *= (-1);
			x += velocityX * dt;
		}
		
		if ( ( y < (maximum*(-1)) ) || ( y > maximum ) ){
			velocityY *= (-1);
			y += velocityY * dt;
		}
		
		//check to see what our maximum|height ( z-value ) is:
		//this calculates the value of our Mexican Hat function and returns z
		//the value of z is the maximum we are looking to find
		//epsilon keeps it from blowing up at 0,0 make it as small as you would like
		current = Math.sin ( Math.sqrt ( x*x + y*y ) ) / ( Math.sqrt( x*x + y*y + epsilon ));
	
		//if we beat our best update best
		if ( current > best ) { 
			best = current; 
			bestX = x;
			bestY = y;
			
		}
		
		//System.out.println ( "x: " + x + ", y: " + y + ", vx " + velocityX + ", vy " + velocityY + ", currentHeight: " + current  ); 
		
	}
	
	
	//use best postition of particle and of group and aim in that direction plus
	// a bit of randomness to keep us from hovering about local maximums
	void updateParticleVelocity ( double groupBestX, double groupBestY )
	{
		velocityX += constantGroup * random.nextDouble() * ( groupBestX - x ) +
					constantParticle * random.nextDouble() * ( bestX - x );
					
		velocityY += constantGroup * random.nextDouble() * ( groupBestY - y ) +
					constantParticle * random.nextDouble() * ( bestY - y );
		
	}
	

}//end class Particle