/*
	Lan Wars.
	Copyright (c) 2009, 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 class performs the most basic level of movement for the AI. It is given an input destination
///				point and a start point and attempts to avoid obstacles in between, performing turning and 
///				returning movement intensity in the process. Path-following is performed by using this class in 
///				conjunction with the path network and a target path.
//=====================================================================================================================

#include "Precompiled.h"
#include "AI/AIMover.h"
#include "Game/GameCore.h"
#include "GameObjects/GameEnemy.h"
#include "Doom/DoomLevel.h"
#include "Doom/DoomGLSubsectorV2.h"
#include "Doom/DoomPPSeg.h"

using namespace Game;

//=====================================================================================================================
/// @brief		Creates the AIMover module
/// @param		level - level the AI will be moving through
/// @pre		The given level cannot be null
//=====================================================================================================================

AIMover::AIMover( const DoomLevel & level ) : m_level(&level)
{
	// Make sure ok
	
	ENSURE( &level != 0 , "The given level cannot be null" )
	
	// Initialise these to the defaults
	
	SetPosition(Vec3f::Zero());
	SetGoalPosition(Vec3f::UnitX());
	SetMovementScale(1);
	SetAngle(0);

	SetAvoidanceRadius(200);
	SetAvoidanceStrength(1.5f);
	SetAvoidanceAttenuation(2.80f);
	SetAvoidanceLimit(0.50f);
	SetTurningStrength(1.5f);
	SetTurningAttenuation(0.45f);
	SetTurningLimit(0.15f);
}

//=====================================================================================================================
/// @brief		Updates the AI's turning and movement.
//=====================================================================================================================

void AIMover::Update()
{
	//-----------------------------------------------------------------------------------------------------------------
	// Turning to reach the goal point
	//-----------------------------------------------------------------------------------------------------------------

	// See how much turning we should do to reach our goal point:

	float goal_turning = EvaluateAttraction(m_goal_position_xz,m_turning_strength,m_turning_attenuation);

	// Get the vector to our goal point:

	Vec2f to_goal = m_goal_position_xz - m_position_xz;

	// Get the angle to our goal point:

	float angle_diff = - Angle::Wrap( ( m_direction_xz ).AngleBetween(to_goal) );

	// Set the turn direction:

	if ( angle_diff < 0 ) goal_turning *= -1;

	// Make sure the turning will not pass over the required angle:

	if ( goal_turning < 0 )
	{
		if ( Float::Abs(goal_turning) > Float::Abs(angle_diff) )
		{
			goal_turning = angle_diff;
		}

		if ( Float::Abs(goal_turning) > m_turning_limit )
		{
			goal_turning = -m_turning_limit;
		}
	}
	else
	{
		if ( goal_turning > angle_diff )
		{
			goal_turning = angle_diff;
		}

		if ( goal_turning > m_turning_limit )
		{
			goal_turning = m_turning_limit;
		}
	}

	//-----------------------------------------------------------------------------------------------------------------
	// Get the total turning to avoid obstacles
	//-----------------------------------------------------------------------------------------------------------------

	//float avoidance_turning = GetAvoidanceTurning();

	//-----------------------------------------------------------------------------------------------------------------
	// Update our angle by the amount of turning:
	//-----------------------------------------------------------------------------------------------------------------

	// Update:

	//SetAngle( GetAngle() + goal_turning + avoidance_turning );
	
	SetAngle( GetAngle() + goal_turning );
}

//=====================================================================================================================
/// @brief		Calcualtes the amount of turning due to collision avoidance.
/// @returns	Amount of turning due to collision avoidance.
/// @pre		GameCore must exist
/// @post		The stack must be in the same position as it was when the function was called
//=====================================================================================================================

float AIMover::GetAvoidanceTurning()
{
	// Make sure ok

	ENSURE( GameCore::GetSingleton() , "GameCore must exist" )

	// Get the game's stack

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

	// Debug only

	#ifdef DEBUG

		void * stack_pos = stack.GetPointer();

	#endif

	//-----------------------------------------------------------------------------------------------------------------
	// Do turning to avoid all segs
	//-----------------------------------------------------------------------------------------------------------------
	
	// Total amount of turning to perform:
	
	float total_turning = 0;
	
	// Find a list of segs surrounding us and put in the stack:

	unsigned seg_count = FindSurroundingSegsRecursive(stack,m_level->GetRootNode());
	
	// Get the direction to our goal point:
	
	Vec2f forward = m_goal_position_xz - m_position_xz; forward.Normalize();
	
	// Get the right vector to this:
	
	Vec2f right = forward.Perpendicular();
	
	// Get the distance of the goal point along our right vector:
	
	float goal_r_dist = ( m_goal_position_xz - m_position_xz ).Dot(right);

	// Run through all the segs:

	for ( unsigned i = 0 ; i < seg_count ; ++i )
	{
		// Get this seg:

		const DoomPPSeg & seg = * ( stack.Pop<const DoomPPSeg *>() );
		
		// Get the center of the seg:
		
		Vec2f seg_center = ( seg.m_start + seg.m_end );
		
		// Get the distance of the seg center along our right vector
		
		float seg_center_r_dist = ( seg_center - m_position_xz ).Dot(right);
		
		// Get the distance of the seg center along our forward vector:
		
		float seg_cetner_f_dist = ( seg_center - m_position_xz ).Dot(forward);
		
		// Only consider the seg if it is in front of us:
		
		if ( seg_cetner_f_dist > 0 )
		{
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			// Calculate the how parallel we are to the seg direction: if we are running parallel to the wall
			// then we want to consider it as less of an obstacle.
			//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
			
			float parallel_scalar = Float::Abs( seg.m_normal.Dot(forward) );
		
			// See whether the seg center is to the left or to the right of our goal point:
			
			if ( seg_center_r_dist < goal_r_dist )
			{
				// Seg center is to the left of our goal point: we should move to the right of it to avoid it.
				
				float repulsion = EvaluateRepulsion
				( 
					seg_cetner_f_dist , m_avoidance_strength , m_avoidance_attenuation 
				);
				
				repulsion *= parallel_scalar;
				
				// Do the turning
				
				total_turning -= repulsion;
			}
			else
			{
				// Seg center is to the right of our goal point: we should move to the left of it to avoid it
				
				float repulsion = EvaluateRepulsion
				( 
					seg_cetner_f_dist , m_avoidance_strength , m_avoidance_attenuation 
				);
				
				repulsion *= parallel_scalar;
				
				// Do the turning
				
				total_turning += repulsion;				
			}
		}
	}
	
	// Make sure the turning is in range:
	
	total_turning = Float::Clamp( total_turning , -m_avoidance_limit , m_avoidance_limit );
	
	// Debug only:

	#ifdef DEBUG

		ENSURE( stack_pos == stack.GetPointer() , "The stack must be in the same position as it was when the function was called" )

	#endif	

	// Return the total amount of turning:
	
	return total_turning;
}

//=====================================================================================================================
/// @brief		Recursively builds a list of segments in the level that surround the AI within it's 
///				obstacle avoidance radius.
/// @param		stack - stack to put all the surrounding segments of interest into
/// @param		node - which node to start this process from in the BSP tree
/// @returns	The number of segs that surround the AI and which are put into the stack
/// @pre		The given node cannot be null
/// @pre		The given stack cannot be null
//=====================================================================================================================

unsigned AIMover::FindSurroundingSegsRecursive( Stack & stack , const DoomPPNode & node )
{
	// Make sure ok
	
	ENSURE( &stack != 0 , "The given stack cannot be null"	)
	ENSURE( &node  != 0 , "The given node cannot be null"	)
	
	// Store the number of segs that were put into the stack here:
	
	unsigned stack_segs = 0;
	
	// Get the distance to the node's plane:
	
	float dist = node.m_normal.Dot(m_position_xz) + node.m_minus_n_dot_p;
	
	// See what side of the plane we are on:
	
	if ( dist >= 0 )
	{
		//-------------------------------------------------------------------------------------------------------------
		// On the right side of the plane
		//-------------------------------------------------------------------------------------------------------------
		
		// See if we are close enough to check the left side also:
		
		if ( dist <= m_avoidance_radius )
		{
			// See if we have a left child node:
			
			if ( node.m_left_child )
			{
				// This should never be the case:
				
				ENSURE( node.m_left_subsector == 0 , "Invalid BSP tree node" )			
			
				// Have a left child: recursively go down through the tree
				
				stack_segs += FindSurroundingSegsRecursive( stack , * node.m_left_child );
			}
			else
			{
				// This should never be the case:
				
				ENSURE( node.m_left_subsector , "Invalid BSP tree node" )
			
				// No left child: this is a leaf. Add all the segs we can collide with in this subsector into the stack
			
				stack_segs += FindSurroundingSegsInSubsector(stack,*node.m_left_subsector);
			}
		}
		
		// See if we have a right child node:
		
		if ( node.m_right_child )
		{
			// This should never be the case:
			
			ENSURE( node.m_right_subsector == 0	, "Invalid BSP tree node" )
			
			// Have a right child: recursively go down through the tree
			
			stack_segs += FindSurroundingSegsRecursive( stack , * node.m_right_child );
		}
		else
		{
			// This should never be the case:
			
			ENSURE( node.m_right_subsector , "Invalid BSP tree node" )
			
			// No right child: this is a leaf. Add all the segs we can collide with in this subsector into the stack
		
			stack_segs += FindSurroundingSegsInSubsector(stack,*node.m_right_subsector);			
		}
	}
	else
	{
		//-------------------------------------------------------------------------------------------------------------
		// On the left side of the plane
		//-------------------------------------------------------------------------------------------------------------
		
		// See if we are close enough to check the right side also:
		
		if ( dist >= - m_avoidance_radius )
		{		
			// See if we have a right child node:
			
			if ( node.m_right_child )
			{
				// This should never be the case:
				
				ENSURE( node.m_right_subsector == 0	, "Invalid BSP tree node" )
				
				// Have a right child: recursively go down through the tree
				
				stack_segs += FindSurroundingSegsRecursive( stack , * node.m_right_child );
			}
			else
			{
				// This should never be the case:
				
				ENSURE( node.m_right_subsector , "Invalid BSP tree node" )
				
				// No right child: this is a leaf. Add all the segs we can collide with in this subsector into the stack
			
				stack_segs += FindSurroundingSegsInSubsector(stack,*node.m_right_subsector);
			}		
		}
		
		// See if we have a left child node:
		
		if ( node.m_left_child )
		{
			// This should never be the case:
			
			ENSURE( node.m_left_subsector == 0 , "Invalid BSP tree node" )
		
			// Have a left child: recursively go down through the tree
			
			stack_segs += FindSurroundingSegsRecursive( stack , * node.m_left_child );
		}
		else
		{
			// This should never be the case:
			
			ENSURE( node.m_left_subsector , "Invalid BSP tree node" )
		
			// No left child: this is a leaf. Add all the segs we can collide with in this subsector into the stack
			
			stack_segs += FindSurroundingSegsInSubsector(stack,*node.m_left_subsector);
		}			
	}
	
	// Return the number of segs that were put into the stack
	
	return stack_segs;
}

//=====================================================================================================================
/// @brief		Adds a list of segs that the AI can collide with in the given subsector into the 
///				given stack. Returns the number of segs added to the stack.
/// @param		stack - stack to put the segs into
/// @param		subsector - subsector we are examining
/// @returns	The number of segs put onto the stack
/// @remarks	The segs are stored as pointers to segs on the stack
//=====================================================================================================================

unsigned AIMover::FindSurroundingSegsInSubsector( Stack & stack , const DoomGLSubsectorV2 & subsector )
{
	// Store the number of segs put onto the stack here
	
	unsigned stack_segs = 0;

	// Get a pointer to the first seg in this subsector:
	
	const DoomPPSeg * seg = & m_level->GetSeg(subsector.m_first_seg);
	
	// Process all the segs in this subsector:
	
	for ( unsigned i = 0 ; i < subsector.m_count ; ++i )
	{
		// Only if the seg is potentially an obstacle:
		
		if ( IsSegPassable(*seg) == false )
		{
			stack_segs++; stack.Push(seg);
		}

		// Move onto the next seg:
		
		seg++;
	}
	
	// Return the number of segs put onto the stack
	
	return stack_segs;
}

//=====================================================================================================================
/// @brief		Tells if the given line segment can be passed by the AI from it's current position
/// @returns	True if the given line segment can be passed by the AI
/// @remarks	This function will always return true if we are on the left side of the seg.
//=====================================================================================================================

bool AIMover::IsSegPassable( const DoomPPSeg & seg )
{
	// See if we are on the right side of the seg:
	
	float dist = seg.m_normal.Dot(m_position_xz) + seg.m_minus_n_dot_p;
	
	// Return true if we are on the left side of the seg:
	
	if ( dist < 0 ) return true;

	// See what type of seg it is:
	
	switch ( seg.m_type )
	{
		//-------------------------------------------------------------------------------------------------------------
		// Full wall:
		//-------------------------------------------------------------------------------------------------------------
		
		case DoomPPSeg::SEG_TYPE_FULL_WALL:
		{
			// Easy case. Full wall. Return false.
			
			return false;
		
		}	break;
		
		//-------------------------------------------------------------------------------------------------------------
		// No wall:
		//-------------------------------------------------------------------------------------------------------------
		
		case DoomPPSeg::SEG_TYPE_NO_WALL:
		{
			// Easy case. No wall. Return true.
			
			return true;
		
		}	break;
		
		//-------------------------------------------------------------------------------------------------------------
		// Lower wall only:
		//-------------------------------------------------------------------------------------------------------------
		
		case DoomPPSeg::SEG_TYPE_LOWER_WALL:
		{
			// Make sure ok:
			
			ENSURE( seg.m_opposite_seg , "INVALID SEG" )
		
			// Only a lower wall can block us. See if that is the case:
			
			if ( seg.m_opposite_seg->m_floor_height - seg.m_floor_height < GameEnemy::COLLISION_HEIGHT * 0.80f )
			{
				// Can pass if only the sector can accomodate our height:
				
				if ( seg.m_opposite_seg->m_ceiling_height - seg.m_opposite_seg->m_floor_height >= GameEnemy::COLLISION_HEIGHT * 2.0f )
				{
					// Can pass
					
					return true;
				}
				else
				{
					// Can't pass
					
					return false;
				}
			}
			else
			{
				// Lower wall too high: return false
				
				return false;
			}
		
		}	break;
		
		//-------------------------------------------------------------------------------------------------------------
		// Upper wall only:
		//-------------------------------------------------------------------------------------------------------------
		
		case DoomPPSeg::SEG_TYPE_UPPER_WALL:
		{
			// Make sure ok:
			
			ENSURE( seg.m_opposite_seg , "INVALID SEG" )
		
			// Only a upper wall can block us. See if that is the case:
			
			if ( seg.m_opposite_seg->m_ceiling_height - seg.m_floor_height < GameEnemy::COLLISION_HEIGHT * 2.0f )
			{
				// Can't pass:
				
				return false;
			}
			else
			{
				// Can pass if only the sector can accomodate our height:
				
				if ( seg.m_opposite_seg->m_ceiling_height - seg.m_opposite_seg->m_floor_height >= GameEnemy::COLLISION_HEIGHT * 2.0f )
				{
					// Can pass
					
					return true;
				}
				else
				{
					// Can't pass
					
					return false;
				}
			}
		
		}	break;
		
		//-------------------------------------------------------------------------------------------------------------
		// Lower and upper wall
		//-------------------------------------------------------------------------------------------------------------
		
		case DoomPPSeg::SEG_TYPE_LOWER_UPPER_WALL:
		{
			// Make sure ok:
			
			ENSURE( seg.m_opposite_seg , "INVALID SEG" )
			
			// Check to see if the lower wall will block us:
			
			if ( seg.m_opposite_seg->m_floor_height - seg.m_floor_height < GameEnemy::COLLISION_HEIGHT * 0.80f )
			{
				// Lower wall is ok: see if the upper wall will block us:
				
				if ( seg.m_opposite_seg->m_ceiling_height - seg.m_floor_height < GameEnemy::COLLISION_HEIGHT * 2.0f )
				{
					// Can't pass:
					
					return false;
				}
				else
				{
					// Can pass if only the sector can accomodate our height:
					
					if ( seg.m_opposite_seg->m_ceiling_height - seg.m_opposite_seg->m_floor_height >= GameEnemy::COLLISION_HEIGHT * 2.0f )
					{
						// Can pass
						
						return true;
					}
					else
					{
						// Can't pass
						
						return false;
					}
				}				
			}
			else
			{
				// Lower wall too high: return false
				
				return false;
			}			
		
		}	break;
	}
	
	// In debug raise an error
	
	ENSURE( seg.m_type == DoomPPSeg::SEG_TYPE_FULL_WALL , "INVALID SEG TYPE" )
	
	// Keep the compiler happy:
	
	return false;
}
