/*
	Trig Wars.
	Copyright (c) 2008, Darragh Coy.
	All rights reserved.
	Original work(s), source code & author(s) details available at: http://www.darraghcoy.com

	Redistribution and use in source and binary forms, with or without modification, are permitted 
	provided that the following conditions are met:

		* 	Redistributions of source code must retain the above copyright notice, this list of 
			conditions and the following disclaimer.
		* 	Redistributions in binary form must reproduce the above copyright notice, this list of 
			conditions and the following disclaimer in the documentation.
		* 	Neither the name of the author nor the names of contributors may be used to endorse or 
			promote products derived from this software without specific prior written permission.
		* 	The use of any third party libraries, modules or data files provided with the source 
			form of the program must follow and respect the individual licensing terms of these 
			components. You do not have permission to modify or circumvent the licensing terms for 
			provided third party components unless the third party licenses specifically allow this. 
			If you do not agree to the terms of provided third party components then you do not have 
			permission to use them in your derived work(s).
		* 	The terms of this license do not apply to separate (but related) distributions if supplied 
			by the author(s), only to the contents of this source distribution, derived works, and the 
			binaries produced by either modified or unmodified versions of the source code. The terms 
			of this license do not also apply to any third party libraries or components provided with 
			the source distribution. Such third party elements are licensed under their own separate 
			agreements.

	THIS SOFTWARE IS PROVIDED BY DARRAGH COY ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
	BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
	ARE DISCLAIMED. IN NO EVENT SHALL DARRAGH COY BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
	EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 
	OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
	OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
	IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

//=====================================================================================================================
/// @brief		This is the base class for all AI managers for the different types of enemies. This is a composite
///				game object which actually holds sub game objects underneath it (AIControlled objects). 
///				This base contains functionality common to all AI managers.
//=====================================================================================================================

#include "Precompiled.h"
#include "GameObjects\AIControlled.h"
#include "GameObjects\AIManager.h"
#include "GameObjects\ObstacleManager.h"
#include "GameObjects\Obstacle.h"
#include "Game\QuadTreeAlgo.h"

using namespace Game;

//=====================================================================================================================
/// @brief		Constructor: creates the AI object
/// @param		manager - AI manager that looks after the object
/// @param		max_health - maximum health this ai object can have
/// @pre		The given manager cannot be null
//=====================================================================================================================

AIControlled::AIControlled( AIManager * manager , float max_health ) : DamageableObject(max_health)
{
	// Make sure ok:

	ENSURE( manager , "The given manager cannot be null" );
	
	// Save manager and add ref to it:
	
	m_manager = manager; m_manager->AddReference();

	// No quad tree entry:

	m_quad_entry = 0;
}				

//=====================================================================================================================
/// @brief		Destructor. Destroys the AI object.
/// @pre		The object should have been removed from its quad tree
//=====================================================================================================================

AIControlled::~AIControlled()
{
	// Done with our manager:
	
	m_manager->RemoveReference();

	// Make sure ok:

	ENSURE( m_quad_entry == 0 , "The object should have been removed from its quad tree" );
}

//=====================================================================================================================
/// @brief		Calculates the amount of turning we should do reach a certain point
/// @param		p - point we want to reach
/// @param		turn_strength - factor controlling the amount of turning that is done
/// @param		distance_attenuation - amount of influence that distance has over turning.
/// @returns	The amount of turning that should be done to reach this point
/// @pre		The given point should be wrapped
/// @pre		Turn strength should be greater than zero
/// @pre		Distance attenuation should be positive
//=====================================================================================================================

float AIControlled::CalculateTurningToPoint( const Vec2f & p , float turn_strength , float distance_attenuation )
{
	// Make sure ok:
	
	ENSURE( turn_strength			> 0		, "Turn strength should be greater than zero"	);
	ENSURE( distance_attenuation	> 0		, "Distance attenuation should be positive"		);
	ENSURE( PositionalObject::IsWrapped(p)	, "Given point should be wrapped."				);
	
	// Need to unwrap the given point:
	
	Vec2f p_unwrapped = PositionalObject::UnwrapCoordinates(GetPosition(),p);
	
	// Get the angle towards the given point:
	
	float angle = GetDirection().FastAngleBetween( p_unwrapped - GetPosition() );	
	
	// Get the distance to the given point
	
	float dist = ( p_unwrapped - GetPosition() ).Length();
	
	// Depending on our angle to the point and how far we are away from it, do some turning
	
	float turning = EvaluatePotentialAttraction( p_unwrapped , angle * turn_strength , dist * distance_attenuation );
	
	// Return the amount of turning to be done:
	
	return turning;
}

//=====================================================================================================================
/// @brief		Flocks this enemy to avoid the given point: returns turning amount to do this
/// @param		avoid_pt - point to avoid
/// @param		view_angle - viewing cone angle of this enemy. This applies to cohesion and alignment behaviours
/// @param		separation_view_angle - viewing cone angle for separation
/// @param		flock_distance - distance or range over which enemies will flock
/// @param		turn_force - affects how much turning will result from this behaviour
/// @param		cohesion_weight - affects how important the cohesion property is for the flock
/// @param		separation_weight - affects how important the separation property is for the flock
/// @param		alignment_weight - affects how important the alignment property is for the flock
/// @param		separation_distance - minimum distance that enemies should be apart
/// @param		separation_attenuation - how much distance to neighbouring entites affects separation
/// @param		maximum_acceleration - maximum amount of acceleration that can be applied
/// @param		leader_percent_accel - amount of acceleration the leader uses
/// @param		minimum_percent_accel - minimum amount of acceleration the factory can apply when following
/// @returns	The amount of turning and acceleration that should be done
/// @pre		The game should exist
/// @pre		View angle should be > 0
/// @pre		Separation angle should be > 0
/// @pre		Turn force should be > 0 
/// @pre		Flock distance should be > 0
/// @pre		Separation distance should be > 0 
/// @pre		Cohesion weight should be >= 0 
/// @pre		Separation weight should be >= 0 
/// @pre		Alignment weight should be >= 0 
/// @pre		Separation attenuation should be >= 0 
/// @pre		Point to avoid should be wrapped
/// @pre		Maximum acceleration should be > 0
/// @pre		Leader percent acceleration should be between 0 and 1
/// @pre		Minimum percent acceleration should be between 0 and 1
//=====================================================================================================================

AIControlled::FlockAction AIControlled::FlockToAvoid
(
	Vec2f avoid_pt					, 
	float view_angle				, 
	float separation_view_angle		,
	float flock_distance			,
	float turn_force				, 
	float cohesion_weight			,
	float separation_weight			,
	float alignment_weight			,
	float separation_distance		,
	float separation_attenuation	,
	float maximum_acceleration		,
	float leader_percent_accel		,	
	float minimum_percent_accel
)
{
	// Make sure ok:

	ENSURE( GameCore::GetSingleton()	, "The game should exist"					);
	ENSURE( flock_distance > 0			, "Flock distance should be > 0"			);
	ENSURE( turn_force > 0				, "Turn force should be > 0 "				);
	ENSURE( view_angle > 0				, "View angle should be > 0 "				);
	ENSURE( separation_view_angle > 0	, "Separation angle should be > 0 "			);
	ENSURE( cohesion_weight >= 0		, "Cohesion weight should be >= 0 "			);
	ENSURE( separation_weight >= 0		, "Separation weight should be >= 0 "		);
	ENSURE( alignment_weight >= 0		, "Alignment weight should be >= 0 "		);
	ENSURE( separation_distance > 0		, "Separation distance should be > 0 "		);
	ENSURE( IsWrapped(avoid_pt)			, "Point to avoid should be wrapped"		);
	ENSURE( maximum_acceleration > 0	, "Maximum acceleration should be > 0"		);
	ENSURE( leader_percent_accel >= 0	, "Leader percent acceleration should be between 0 and 1"	);
	ENSURE( leader_percent_accel <= 1	, "Leader percent acceleration should be between 0 and 1"	);
	ENSURE( minimum_percent_accel >= 0	, "Minimum percent acceleration should be between 0 and 1"	);
	ENSURE( minimum_percent_accel <= 1	, "Minimum percent acceleration should be between 0 and 1"	);
	
	// Get the angle for both vectors of our viewing cone: do for both the cohesion and alignment view cone and the separation view cone

	float view_cone_a1 = GetAngle() + view_angle * 0.5f;
	float view_cone_a2 = GetAngle() - view_angle * 0.5f;
	
	float separation_view_cone_a1 = GetAngle() + separation_view_angle * 0.5f;
	float separation_view_cone_a2 = GetAngle() - separation_view_angle * 0.5f;	

	// Get the directions for both view cone vectors:

	Vec2f vcd1 = Vec2f( Float::Cos(view_cone_a1) , Float::Sin(view_cone_a1) );
	Vec2f vcd2 = Vec2f( Float::Cos(view_cone_a2) , Float::Sin(view_cone_a2) );

	Vec2f separation_vcd1 = Vec2f( Float::Cos(separation_view_cone_a1) , Float::Sin(separation_view_cone_a1) );
	Vec2f separation_vcd2 = Vec2f( Float::Cos(separation_view_cone_a2) , Float::Sin(separation_view_cone_a2) );

	// Get our position:

	Vec2f pos = GetPosition();
	
	// We need to unwrap this point for it to work ok across map boundaries
	
	avoid_pt = UnwrapCoordinates( pos , avoid_pt );	

	// Now determine the minimum x and y value for a bounding box in the search area around us

	Vec2f search_bb_min = pos - flock_distance;
	Vec2f search_bb_max = pos + flock_distance;

	//=================================================================================================================
	// Search for flock neigbours
	//=================================================================================================================

	// Get the stack from the game:

	Stack & stack = GameCore::GetSingleton()->GetStack();

	// Do a search in our quad tree around us for neighbours

	unsigned num_results = QuadTreeAlgo::FindInRectangle
	( 
		GetAIManager().GetQuadTree()	, 
		stack							,
		search_bb_min					,
		search_bb_max - search_bb_min
	);
	
	// Store the amount of enemies that are within our viewing cone here

	unsigned in_view_cone = 0;

	// Store the average local position of our neighbours here:

	Vec2f avg_pos_local = Vec2f::Zero();

	// Store the average heading of our neighbours here:

	Vec2f avg_heading = Vec2f::Zero();

	// This is point we want to turn to in order to separate

	Vec2f separate_point_local = Vec2f::Zero();

	// Get the normals to both view cone vectors: we'll use these to determine if stuff is inside our viewing cone

	Vec2f vcn1 =  vcd1.Perpendicular();
	Vec2f vcn2 = -vcd2.Perpendicular();
	
	Vec2f separation_vcn1 =  vcd1.Perpendicular();
	Vec2f separation_vcn2 = -vcd2.Perpendicular();
	
	// Store a weighted distance to all objects we want to separate from here
	
	float weighted_sep_distance = 0;
	
	// Store the total weight for all the objects we want to separate from here
	
	float total_separation_weight = 0;

	//=================================================================================================================
	// Run through all the enemies we found nearby:
	//=================================================================================================================

	for ( unsigned i = 0 ; i < num_results ; ++i )
	{
		// Get this object:

		AIControlled * obj = stack.Pop<AIControlled*>();

		// Make sure this is not the same object as us: if so then carry on

		if ( obj == this ) continue;

		// Get it's position relative to ours:

		Vec2f relative = UnwrapCoordinates( pos , obj->GetPosition() ) - pos;
		
		// Get the square distance to this object: 
		
		float sq_dist_to = relative.SqLength();
		
		// Make sure it is within view cone radius: if not then ignore
		
		if ( sq_dist_to > Float::Square(flock_distance) ) continue;

		// Do a dot with the view cone normals to see if within our view cone

		if ( relative.Dot(vcn1) >= 0 && relative.Dot(vcn2) >= 0 )
		{
			// Increase number of things in our view cone

			in_view_cone++;

			// Within our view cone: add to average position

			avg_pos_local += relative;

			// Add to average heading:

			avg_heading += obj->GetLinearVelocity();
		}
		
		// See if within distance for separation:	

		if ( sq_dist_to < Float::Square(separation_distance) )
		{
			// See if within the separation view cone:
			
			if ( relative.Dot(separation_vcn1) >= 0 && relative.Dot(separation_vcn2) >= 0 )
			{		
				// Need to seprate: get the real distance

				float dist = relative.Length();

				// Scale separation with distance

				float sep_scale = 1.0f / ( dist * separation_attenuation + 0.00001f );	
				
				// Get the dot of our velocity with the relative vector: only slow down if moving towards the separation point
				
				if ( relative.Dot(GetLinearVelocity()) > 0 )
				{						
					// Add to the total weighted separation distance:
					
					total_separation_weight += sep_scale; weighted_sep_distance += dist * sep_scale;
					
					// Normalise the realative vector
					
					relative /= dist + 0.00001f;
					
					// Add to the point we will steer away from:
					
					separate_point_local -= relative * sep_scale;
				}
			}
		}		
	}

	//=================================================================================================================
	// Now see if we have found anything else in the flock:
	//=================================================================================================================

	if ( in_view_cone > 0 )
	{
		// Not on our own: calculate turning due to alignment, cohesion and separation

		float cohesion_turning		= 0;
		float alignment_turning		= 0;
		float separation_turning	= 0;

		// Do alignment first:

		{
			// Get the average heading of all of the entites

			avg_heading /= (float) in_view_cone;

			// Get the angle we have to turn to make this heading

			alignment_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , avg_heading.GetAngle() );
		}

		// Do cohesion next:

		{		
			// Get the average position of all our neighbours

			avg_pos_local /= (float) in_view_cone;

			// Turn towards that position

			cohesion_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , avg_pos_local.GetAngle() );
		}	
		
		// Do separation:

		{		
			// Turn towards the separation point

			separation_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , separate_point_local.GetAngle() );
		}		

		// Makeup the total turning to be done:

		float turning = 
		(	
			alignment_turning	* alignment_weight	+ 
			separation_turning	* separation_weight + 
			cohesion_turning	* cohesion_weight
		)		
		* turn_force;
		
		// See what percent of movement speed we are to move at:
		
		float move_speed = maximum_acceleration;
		
		if ( total_separation_weight > 0 )
		{
			// Must separate from other objects: therefore must slow down also. Calculate the average distance to other objects
			
			float avg_dist = weighted_sep_distance / total_separation_weight;
			
			// Normalise within the range of separation distance:
			
			avg_dist /= separation_distance;
			
			// Make sure from within range
			
			avg_dist = Float::Clamp( avg_dist , minimum_percent_accel , 1 );
			
			// Scale speed accordingly
			
			move_speed *= avg_dist;
		}
		
		// Now makeup the flock action and return
		
		FlockAction a; a.m_steering = turning; a.m_acceleration = move_speed;
		
		// Return 
		
		return a;
	}
	else
	{
		// Makeup the flocking action:
		
		FlockAction a;
		
		// We move at specified leader speed
		
		a.m_acceleration = leader_percent_accel * maximum_acceleration;	
	
		// We're on our own: get vector from the point we're trying to avoid to us

		Vec2f from_avoid_pt = pos - avoid_pt;

		// Get the angle of this vector:
		
		float turn_to_angle = from_avoid_pt.GetAngle();	

		// Turn towards that angle:

		a.m_steering = Angle::TurnTo( Float::TwoPI() , GetAngle() , turn_to_angle ) * turn_force;
		
		// Return the steering
		
		return a;
	}
}

/*
float AIControlled::FlockToAvoid
(
	Vec2f avoid_pt				, 
	float view_angle			, 
	float flock_distance		,
	float turn_force			, 
	float cohesion_weight		,
	float separation_weight		,
	float alignment_weight		,
	float separation_distance	,
	float separation_attenuation
)
{
	// Make sure ok:

	ENSURE( GameCore::GetSingleton()	, "The game should exist"				);
	ENSURE( flock_distance > 0			, "Flock distance should be > 0"		);
	ENSURE( turn_force > 0				, "Turn force should be > 0 "			);
	ENSURE( view_angle > 0				, "View angle should be > 0 "			);
	ENSURE( cohesion_weight >= 0		, "Cohesion weight should be >= 0 "		);
	ENSURE( separation_weight >= 0		, "Separation weight should be >= 0 "	);
	ENSURE( alignment_weight >= 0		, "Alignment weight should be >= 0 "	);
	ENSURE( separation_distance > 0		, "Separation distance should be > 0 "	);
	ENSURE( IsWrapped(avoid_pt)			, "Point to avoid should be wrapped"	);

	// Get the angle for both vectors of our viewing cone:

	float view_cone_a1 = GetAngle() + view_angle * 0.5f;
	float view_cone_a2 = GetAngle() - view_angle * 0.5f;

	// Get the directions for both view cone vectors:

	Vec2f vcd1 = Vec2f( Float::Cos(view_cone_a1) , Float::Sin(view_cone_a1) );
	Vec2f vcd2 = Vec2f( Float::Cos(view_cone_a2) , Float::Sin(view_cone_a2) );

	// Determine points at the end of the viewing cone:

	Vec2f vc_p1 = vcd1 * flock_distance;
	Vec2f vc_p2 = vcd2 * flock_distance;

	// Get our position:

	Vec2f pos = GetPosition();
	
	// We need to unwrap this point for it to work ok across map boundaries
	
	avoid_pt = UnwrapCoordinates( pos , avoid_pt );	

	// Now determine the minimum x and y value for a bounding box encapulsating the view cone points and our pos

	Vec2f search_bb_min = pos;
	Vec2f search_bb_max = pos;

	if ( vc_p1.x < search_bb_min.x ) search_bb_min.x = vc_p1.x; else if ( vc_p1.x > search_bb_max.x ) search_bb_max.x = vc_p1.x;
	if ( vc_p2.x < search_bb_min.x ) search_bb_min.x = vc_p2.x; else if ( vc_p2.x > search_bb_max.x ) search_bb_max.x = vc_p2.x;
	if ( vc_p1.y < search_bb_min.y ) search_bb_min.y = vc_p1.y; else if ( vc_p1.y > search_bb_max.y ) search_bb_max.y = vc_p1.y;
	if ( vc_p2.y < search_bb_min.y ) search_bb_min.y = vc_p2.y; else if ( vc_p2.y > search_bb_max.y ) search_bb_max.y = vc_p2.y;
	
	// Need to also use our own direction in some cases as the bb may be too small (such as when our dir = 0 and view cone is 180 degrees for example)
	
	Vec2f dir = GetDirection() * flock_distance;
	
	if ( dir.x < search_bb_min.x ) search_bb_min.x = dir.x; else if ( dir.x > search_bb_max.x ) search_bb_max.x = dir.x;
	if ( dir.x < search_bb_min.x ) search_bb_min.x = dir.x; else if ( dir.x > search_bb_max.x ) search_bb_max.x = dir.x;

	//=================================================================================================================
	// Search for flock neigbours
	//=================================================================================================================

	// Get the stack from the game:

	Stack & stack = GameCore::GetSingleton()->GetStack();

	// Do a search in our quad tree around us for neighbours

	unsigned num_results = QuadTreeAlgo::FindInRectangle
	( 
		GetAIManager().GetQuadTree()	, 
		stack							,
		search_bb_min					,
		search_bb_max - search_bb_min
	);
	
	// Store the amount of enemis that are within our viewing cone here

	unsigned in_view_cone = 0;

	// Store the average position of our neighbours here:

	Vec2f avg_pos = Vec2f::Zero();

	// Store the average heading of our neighbours here:

	Vec2f avg_heading = Vec2f::Zero();

	// This is the amount of turning to do for separation

	float separation_turning = 0;

	// Get the normals to both view cone vectors: we'll use these to determine if stuff is inside our viewing cone

	Vec2f vcn1 =  vcd1.Perpendicular();
	Vec2f vcn2 = -vcd2.Perpendicular();

	//=================================================================================================================
	// Run through all the enemies we found nearby:
	//=================================================================================================================

	for ( unsigned i = 0 ; i < num_results ; ++i )
	{
		// Get this object:

		PositionalObject * obj = stack.Pop<PositionalObject*>();

		// Make sure this is not the same object as us: if so then carry on

		if ( obj == this ) continue;

		// Get the position of this object:

		Vec2f obj_pos = obj->GetPosition();
		
		// This needs to be unwrapped
		
		obj_pos = UnwrapCoordinates(pos,obj_pos);

		// Get it's position relative to ours:

		Vec2f relative = obj_pos - pos;		

		// Do a dot with the view cone normals to see if within our view cone

		if ( relative.Dot(vcn1) >= 0 && relative.Dot(vcn2) >= 0 )
		{
			// Get the square distance to this object: make sure it is in the view cone
			
			float sq_dist_to =  relative.SqLength();
			
			// Make sure it is within view cone radius
			
			if ( sq_dist_to > Float::Square(flock_distance) ) continue;
		
			// Increase number of things in our view cone

			in_view_cone++;

			// Within our view cone: add to average position

			avg_pos += obj_pos;

			// Add to average heading:

			avg_heading += obj->GetDirection();

			// Get the square distance between us and this object: see if we need to separate

			if ( sq_dist_to < Float::Square(separation_distance) )
			{
				// Need to seprate: get the real distance

				float dist = relative.Length();

				// Scale separation with distance

				float sep_scale = 1.0f - ( ( separation_distance - dist ) / separation_distance );

				// Get a vector from the enemy to us:

				Vec2f to_us = pos - obj_pos;

				// Get the angle that this vector makes: this vector points away from the enemy

				float to_us_ang = to_us.GetAngle();

				// Turn towards that angle, more so as we get closer

				separation_turning += Angle::TurnTo( Float::TwoPI() , GetAngle() , to_us_ang ) * Float::Pow( sep_scale , separation_attenuation );
			}
		}
	}

	//=================================================================================================================
	// Now see if we have found anything else in the flock:
	//=================================================================================================================

	if ( in_view_cone > 0 )
	{
		// Not on our own: calculate turning due to alignment and cohesion

		float cohesion_turning = 0;
		float alignment_turning = 0;

		// Do alignment first:

		{
			// Get the average heading of all of the entites

			avg_heading /= (float) in_view_cone;

			// Get the angle we have to turn to make this heading

			alignment_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , avg_heading.GetAngle() );
		}

		// Do cohesion next:

		{		
			// Get the average position of all our neighbours

			avg_pos /= (float) in_view_cone;

			// Get a vector from us to that position:

			Vec2f to_avg_pos = avg_pos - pos;

			// Turn towards that position

			cohesion_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , to_avg_pos.GetAngle() );
		}

		// Return the total turning to be done:

		return 
		(
			(	alignment_turning	* alignment_weight	+ 
				separation_turning	* separation_weight + 
				cohesion_turning	* cohesion_weight
			)
			
			* turn_force
		);
	}
	else
	{
		// We're on our own: get vector from the point we're trying to avoid to us

		Vec2f from_avoid_pt = pos - avoid_pt;

		// Get the angle of this vector:
		
		float turn_to_angle = from_avoid_pt.GetAngle();

		// Turn towards that angle:

		return Angle::TurnTo( Float::TwoPI() , GetAngle() , turn_to_angle ) * turn_force;
	}
}*/

//=====================================================================================================================
/// @brief		Swarms this enemy to move towards the given point: returns turning amount to do this
/// @param		chase_pt - point we want to chase towards
/// @param		turn_force - force which we can use to turn with
/// @param		swarm_sq_area - half extents of the area around the ship which it looks for other enemies to swarm with
/// @param		attraction_force - influences swarming and how tightly enemies clump together
/// @param		repulsion_force - influences swarming and how close together enemies can get
/// @param		attraction_attenuation - influences distance's effect on swarm attraction
/// @param		repulsion_attenuation - influences distance's effect on swarm repulsion
/// @param		swarm_influence - multiplier that controls the overal influence the swarming behaviour has in the 
///				overall behaviour. The overall behaviour consists of swarming and chasing (the player).
/// @returns	The amount of turning that the AI character should do
/// @pre		The game should exist
/// @pre		Swarm area should be > 0
/// @pre		Attraction force should be >= 0
/// @pre		Repulsion force should be >= 0
/// @pre		Attraction attenuation should be >= 0
/// @pre		Repulsion attenuation should be >= 0
/// @pre		Swarm influnece should be >= 0
/// @pre		Point to chase should be wrapped to map coordinates
//=====================================================================================================================

float AIControlled::SwarmToChase
(
	Vec2f chase_pt					, 
	float turn_force				, 
	float swarm_sq_area				,
	float attraction_force			,
	float repulsion_force			,
	float attraction_attenuation	,
	float repulsion_attenuation		,
	float swarm_influence
)
{
	// Make sure ok:

	ENSURE( GameCore::GetSingleton()		, "The game should exist"								);
	ENSURE( swarm_sq_area > 0				, "Swarm area should be > 0"							);
	ENSURE( attraction_force >= 0			, "Attraction force should be >= 0"						);
	ENSURE( repulsion_force >= 0			, "Repulsion force should be >= 0"						);
	ENSURE( attraction_attenuation >= 0		, "Attraction attenuation should be >= 0"				);
	ENSURE( repulsion_attenuation >= 0		, "Repulsion attenuation should be >= 0"				);
	ENSURE( swarm_influence >= 0			, "Swarm influnece should be >= 0"						);
	ENSURE( IsWrapped(chase_pt)				, "Point to chase should be wrapped to map coordinates"	);
	
	// Get the stack from the game:
	
	Stack & stack = GameCore::GetSingleton()->GetStack();
	
	// Get our position:
	
	Vec2f pos = GetPosition();
	
	// We need to unwrap this for it to work ok across map boundaries
	
	chase_pt = UnwrapCoordinates(pos,chase_pt);
	
	// Right: do a search for other nearby enemies of this type
	
	unsigned nearby_enemies = QuadTreeAlgo::FindInRectangle
	( 
		GetAIManager().GetQuadTree()					, 
		stack											,
		pos - swarm_sq_area								,
		Vec2f(swarm_sq_area*2.0f,swarm_sq_area*2.0f)
	);
	
	// Store the total amount of turning due to swarming here:
	
	float swarm_turning = 0;

	// Run through all the nearby enemies we found:
	
	for ( unsigned i = 0 ; i < nearby_enemies ; ++i )
	{
		// Get this object:
		
		PositionalObject * obj = stack.Pop<PositionalObject*>();
		
		// Do nothing if it's us:
		
		if ( obj == this ) continue;
		
		// Get the position of the object
		
		Vec2f obj_pos = obj->GetPosition();
		
		// Wee need to unwrap this position:
		
		obj_pos = UnwrapCoordinates(pos,obj_pos);
	
		// Do the lenard-jones potential function with this object:
		
		float potential = EvaluatePotential
		( 
			obj_pos					,
			attraction_force		,
			repulsion_force			,
			attraction_attenuation	,
			repulsion_attenuation
		);
		
		// Calculate how much we need to turn towards this object:
		
		float turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , ( obj_pos - pos ).GetAngle() );
		
		// Now add to the total turning by it's potential:
		
		swarm_turning += turning * potential;
	}
	
	// Calculate the turning towards our goal point:
	
	float goal_turning = Angle::TurnTo( Float::TwoPI() , GetAngle() , ( chase_pt - pos ).GetAngle() );
	
	// Return the amount of turning to be done:
	
	return ( goal_turning + swarm_turning * swarm_influence ) * turn_force;
}

//=====================================================================================================================
/// @brief		Makes this enemy avoid obstacles: returns turning amount to do this
/// @param		scan_radius - radius around the object in which it scans for obstacles
/// @param		repulsion_strength - the strength of repulsion
/// @param		repulsion_attenuation - how much distance affects repulsion
/// @param		max_turning - maximum amount of turning or angular acceleration that can be done
/// @param		time_delta - time between this frame and the last
/// @pre		The game must exist
/// @pre		Max turning should be > 0
/// @pre		Time delta should be >= 0
/// @pre		Scan radius must be > 0
/// @post		The stack used by the function must have it's stack pointer unchanged by the function end
//=====================================================================================================================

void AIControlled::AvoidObstacles
(
	float scan_radius				,
	float repulsion_strength		,
	float repulsion_attenuation		,
	float max_turning				,
	float time_delta			
)
{
	// Make sure ok:
	
	ENSURE( GameCore::GetSingleton()	, "The game must exist"			);
	ENSURE( max_turning > 0				, "Max turning should be > 0"	);
	ENSURE( time_delta >= 0				, "Time delta should be >= 0"	);
	ENSURE( scan_radius > 0				, "Scan radius must be > 0"		);
	
	// Get the game:
	
	GameCore * game = GameCore::GetSingleton();

	// Get the obstacle manager:
	
	ObstacleManager * mgr = GetAIManager().GetObstacleManager();
	
	// See if there:
	
	if ( mgr )
	{
		// Get the two stacks from the game
		
		Stack & s1 = game->GetStack();
		Stack & s2 = game->GetSecondaryStack();
		
		// Debug only:
		
		#ifdef DEBUG
		
			void * stack_ptr = s1.GetPointer();
		
		#endif
		
		// Do collision with obstacles, using the scan radius as our bounding cirlce radiis
		
		unsigned num_results = mgr->ObstacleCollide( GetPosition() , scan_radius , s1 , s2 );
		
		// Run through all the results:
			
		for ( unsigned i = num_results ; i != 0 ; --i )
		{
			// Get this obstacle off the stack:
			
			Obstacle * o = s1.Pop<Obstacle*>();
			
			// Get the position of the obstacle and unwrap it:
			
			Vec2f obs_pos_unwrapped = PositionalObject::UnwrapCoordinates( GetPosition() , o->GetPosition() );
			
			// Get how far away we are from it's center:
			
			float dist = obs_pos_unwrapped.FastDistanceTo( GetPosition() );
			
			// See how close we are to the outer radius of the obstacle:
			
			float dist_to_radius = dist - o->GetRadius();
			
			// Make sure it is not less than zero:
			
			if ( dist_to_radius < 0 ) dist_to_radius = 0;
			
			// Get the potential value for this obstacle: use the distance to the radius because we really want to avoid collision with the outer radius more than anything
			
			float potential = -EvaluatePotentialRepulsion
			( 
				dist_to_radius			,	// Distance to the point being evaluated. 
				repulsion_strength		,	// repulsion strength
				repulsion_attenuation		// repulsion attenuation
			);
			
			// Get the angle from the obstacle to us:
			
			float from_o_angle = ( GetPosition() - obs_pos_unwrapped ).GetAngle();
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Now we want to turn at right angles to this obstacle to move around it: get an angle -90 degrees 
			// and +90 degrees to the angle to the obstacle and see which is closer:
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			// This is the angle we will turn to:
			
			float turn_to_angle;						
			
			{			
				// Get the two angles we could move in a perpendicular direction away from the obstacle
			
				float right_angle1 = from_o_angle - ( Float::PI() * 0.5f );
				float right_angle2 = from_o_angle + ( Float::PI() * 0.5f );
				
				// See which is closer to our current orientation: we want to turn the least amount
				
				if 
				( 
					Float::Abs( Angle::Difference(GetAngle(),right_angle1) ) > 
					Float::Abs( Angle::Difference(GetAngle(),right_angle2) ) 
				)
				{
					turn_to_angle = right_angle2;
				}
				else
				{
					turn_to_angle = right_angle1;
				}
			}
			
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Now: depending on how close we are to the radius of the obstacle, turn away 
			// directly from it or move at right angles to the obstacle. Interpolate between
			// these two angles of movement depending on how close we are to the obstacle.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			{
				// Interpolate between moving at right angles to the obstacle and moving away from it:
				
				float t = dist / scan_radius;
				
				// Make sure not > 1
				
				if ( t > 1 ) t = 1;
				
				// Do the interpolation:
				
				turn_to_angle *= t; turn_to_angle += ( 1.0f - t ) * from_o_angle;
			}
			
			// Calculate how much turning we need to do to face this direction and bring in 0-PI range
			
			float turn_angle = Angle::TurnTo( Float::TwoPI() , GetAngle() , turn_to_angle );
			
			// Scale with potential function
			
			turn_angle *= potential;
			
			// Clamp the amount of turning:
			
			float turning = Float::Clamp( turn_angle , -max_turning , max_turning );
			
			// Do the turning:
			
			AngularAcclerate( turning , time_delta );
		}
		
		// Debug only:
		
		#ifdef DEBUG
		
			ENSURE( s1.GetPointer() == stack_ptr , "The stack used by the function must have it's stack pointer unchanged by the function end" )
		
		#endif		
	}
}