//////////////////////////////////////////////////////////////////////////////
//
// AVP_Sense_MotionTracker.cpp
//
// Created by NJD - 06 December 2006
//
// Copyright (C) 2006 Rebellion, All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

// I N C L U D E S ///////////////////////////////////////////////////////////

// Core
#include "Core/Asura.h"

// This files header
#include "Knowledge/SenseTypes/AVP_Sense_MotionTracker.h"

#include "Debugging/Asura_DebugLineRender.h"

// Asura
#include "Blueprint/Asura_Blueprint.h"
#include "Blueprint/Asura_Blueprint_System.h"
#include "ChunkSystem/Asura_Chunk_Stream.h"
#include "Entity/PhysicalObject/Moves/Asura_Movement_System.h"

#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Rendering/Asura_Font_System.h"
#endif

// Axon
#include "Core/Axon_Project_System.h"
#include "Core/AVP_Waypoint_Manager.h"
#include "Knowledge/AVP_Knowledge_System_Actor.h"
#include "Knowledge/AVP_Knowledge_System_Sentrygun.h"
#include "Knowledge/Axon_Knowledge.h"
#include "Knowledge/SenseTypes/Axon_Sense_Types.h"
#include "Tactical/AVP_Tactical_System.h"
#include "Core/BrainTypes/AVP_Brain_Actor_Civilian.h"
#include "Core/BrainTypes/AVP_Brain_Actor_Marine.h"
#include "Core/BrainTypes/AVP_Brain_Actor_SentryGun.h"

// AVP
#include "Entity/AVP_Entity_Classification.h"
#include "Entity/MotionTrackerBlip/AVP_ServerEntity_MotionTrackerBlip.h"
#include "Entity/PhysicalObject/Actor/AVP_ClientEntity_Actor.h"
#include "Entity/PhysicalObject/Actor/AVP_ServerEntity_Actor.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Predator/AVP_ServerActor_Predator.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Human/AVP_ClientActor_Human.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Human/AVP_ServerActor_Human.h"

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Sense_MotionTracker
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Sense_MotionTracker::s_fVelocity = 0.0f;

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: Constructor
//---------------------------------------------------------------------------
AVP_Sense_MotionTracker::AVP_Sense_MotionTracker(AVP_Knowledge_System_Actor* pxAVPKnowledgeSystem)
: Axon_Sense< Axon_Knowledge >( pxAVPKnowledgeSystem )
, m_fMTRange( 30.0f )
, m_fDetectableVelocity( 0.5f )
{
	m_fTanFovX     = Asura_Maths::Tangent(Asura_Maths::DegToRad(179.0f) * 0.5f);
	m_fTanFovYUp   = Asura_Maths::Tangent(Asura_Maths::DegToRad(179.0f) * 0.25f);
	m_fTanFovYDown = Asura_Maths::Tangent(Asura_Maths::DegToRad(179.0f) * 0.25f);
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: Deconstructor
//---------------------------------------------------------------------------
AVP_Sense_MotionTracker::~AVP_Sense_MotionTracker() 
{
	DestroyAllKnowledge();
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: DestroyAllKnowledge
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::DestroyAllKnowledge()
{
	Asura_Hashed_List_Deleter_It< Axon_Knowledge > xMTListIter( &m_xMTList );
	
	while ( !xMTListIter.Done() )
	{
		Axon_Knowledge* pxToDelete = xMTListIter.GetCurrent();

		if ( pxToDelete )
		{
			xMTListIter.DeleteCurrent();
			m_pxKnowledgeSystem->DestroyKnowledge( pxToDelete );
		}
	}
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: InitialiseFromBlueprint
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::InitialiseFromBlueprint(const Asura_Blueprint& xBlueprint)
{
	InitFromSettingsBlueprint();
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ReadFromChunkStream
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::ReadFromChunkStream(Asura_Chunk_Stream& xStream)
{
	int iVersion;
	xStream >> iVersion;

	//PARENT::ReadFromChunkStream(xStream);

	Asura_Assertf(iVersion == CURRENT_CHUNK_VERSION, ("AVP_Sense_MotionTracker::ReadFromChunkStream - unhandled chunk version (%d)", iVersion));
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: WriteToChunkStream
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::WriteToChunkStream(Asura_Chunk_Stream& xStream) const
{
	xStream << CURRENT_CHUNK_VERSION;

	//PARENT::WriteToChunkStream(xStream);
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: Update
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::Update(const float fTimeslice) 
{
	AVP_ServerActor_Human* pxServerActor = static_cast<AVP_ServerActor_Human*>( GetActorKnowledgeSystem()->GetActorBrain()->GetActor() );
	AVP_ClientEntity_Actor* pxClient = AVP_ClientEntity_Actor::FindEntity(pxServerActor->GetGuid());

	// For cases of spawned actors make sure we have started to render them before trying to use them for knowledge.
	if (pxClient && pxClient->GetShapeInstance().GetAnimation() && pxClient->GetShapeInstance().GetAnimation()->GetTweenStore())
	{
		if (pxClient->GetShapeInstance().GetAnimation()->GetTweenStore()->IsPositionDataValid())
		{
			Asura_Matrix_3x3 xOri;

			pxServerActor->GetChestOrientation(xOri);
			m_xDirection = Asura_Vector_3( xOri.e13, xOri.e23, xOri.e33 );
			DepricateCurrentKnowledge(fTimeslice);
			AddNewKnowledge();
		}
	}
}

DEBUGVAR bool g_RenderSpeed = false;

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: DepricateCurrentKnowledge
//---------------------------------------------------------------------------
#ifndef ASURA_NO_DEBUG_OUTPUT
void AVP_Sense_MotionTracker::RenderKnowledgeDebugInfo()
{
	if ( g_RenderSpeed )
	{
		Asura_Vector_2		xPos			= Asura_Vector_2(20.0f, 200.0f);
		u_int				uFlags			= ASURA_TEXTFORMAT_LEFTJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING;

		Asura_Font_System::Print( xPos.x, xPos.y, 0.5f, 0xffffffff,	uFlags,	"Velocity %0.2f", s_fVelocity );
	}
}
#endif

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: DepricateCurrentKnowledge
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::DepricateCurrentKnowledge(const float fTimeslice)
{
	AVP_ServerEntity_Actor* pxServerActor = GetActorKnowledgeSystem()->GetActorBrain()->GetActor();

	// Age the vision knowledge. Cull any that are too old
	Asura_Hashed_List_Deleter_It<Axon_Knowledge> xMTIter(&m_xMTList);

	while (!xMTIter.Done())
	{
		Axon_Knowledge* pxKnowledge = xMTIter.GetCurrent();
		const AVP_ServerEntity_Actor* pxActor = AVP_ServerEntity_Actor::FindEntity(pxKnowledge->GetGuid());

		bool bDeleteKnowledge = false;

		// Actor validation, pointer check, dead check is active check
		if (!pxActor || pxActor->IsDead() || !pxActor->IsEntityActive())
		{
			bDeleteKnowledge = true;
		}

		if ( pxActor )
		{
			if (AVP_Tactical_System::GetAllegiance(*pxServerActor, *pxActor) != AVP_ALLEGIANCE_ENEMY)
			{
				bDeleteKnowledge = true;
			}
		}

		// Knowledge time out
		pxKnowledge->m_fAge += fTimeslice;

		if (pxKnowledge->m_fAge >= 5.0f)
		{
			bDeleteKnowledge = true;
		}

		if (bDeleteKnowledge)
		{
			xMTIter.DeleteCurrent();
			m_pxKnowledgeSystem->DestroyKnowledge(pxKnowledge);
		}
		else 
		{
			xMTIter.Next();
		}
	}
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: AddNewKnowledge
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::AddNewKnowledge(void)
{
	ProcessActors();
	ProcessMotionTrackerBlips();
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ProcessActors
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::ProcessActors( void )
{
	AVP_ServerEntity_Actor* pxServerActor = GetActorKnowledgeSystem()->GetActorBrain()->GetActor();

	for (Asura_Hashed_List_ConstIt<AVP_ServerEntity_Actor> xIt(AVP_ServerEntity_Actor::GetList()); !xIt.Done(); xIt.Next())
	{
		const AVP_ServerEntity_Actor*	pxActor = xIt.GetCurrent();		

		// isActor Valid
		if (!pxActor)
			continue;

		// Ignore ourselves.
		if (pxServerActor->GetGuid() == pxActor->GetGuid())
			continue;

		// is the actor dead or inactive, don't remove from the list as he will be removed next time
		if (pxActor->IsDead() || !pxActor->IsEntityActive())
			continue;

		if ( GetActorKnowledgeSystem()->GetTargetPriority( pxActor->GetGuid() ) == AVP_AI_TargetPriorityData::IGNORE_P )
			continue;

		if ( AVP_Tactical_System::GetAllegiance(*pxServerActor, *pxActor) != AVP_ALLEGIANCE_ENEMY)
		{
			continue;
		}

		if ( ShouldCullOnRangeAndFOV( pxServerActor, pxActor->GetPosition() ) )
		{
			continue;
		}

		Asura_Vector_3 xVelocity = pxActor->GetVelocity();
		float fVelocitySq = xVelocity.MagnitudeSquared();

		if ( fVelocitySq <= m_fDetectableVelocity * m_fDetectableVelocity )
		{
			continue;
		}

		s_fVelocity = Asura_Maths::Sqrt( fVelocitySq );

		// Now check to see if an entry already exists.
		Axon_Knowledge* pxExistingKnowledge = m_xMTList.FindObject(pxActor->GetGuid());

		if (pxExistingKnowledge)
		{
			pxExistingKnowledge->m_xDirection		= pxActor->GetPosition() - pxServerActor->GetPosition();
			pxExistingKnowledge->m_fDistance		= pxExistingKnowledge->m_xDirection.Normalise();
		}
		else
		{
			// Create a new one.
			Axon_Knowledge* pxKnowledge = m_pxKnowledgeSystem->CreateKnowledge();
			if (pxKnowledge)
			{
				GetActorKnowledgeSystem()->ResetAlertTimer();

				pxActor->GetChestPosition( pxKnowledge->m_xPosition );

				pxKnowledge->m_uKnowledgeID          = m_pxKnowledgeSystem->GetNewKnowledgeID();
				pxKnowledge->m_uSourceGuid           = pxActor->GetGuid();
				pxKnowledge->m_uSourceClassification = pxActor->GetClassification();
				pxKnowledge->m_iSenseID              = AVP_SENSE_TYPE_MOTION_TRACKER;
				pxKnowledge->m_uTypeInformation		 = AXON_SOUND_TYPE_FOOTSTEP;
				pxKnowledge->m_fAge                  = 0.0f;
				pxKnowledge->m_fThreatValue          = 0.0f;
				pxKnowledge->m_xDirection            = pxActor->GetPosition() - pxServerActor->GetPosition();
				pxKnowledge->m_fDistance             = pxKnowledge->m_xDirection.Normalise();

				// Add it to the knowledge list.
				m_xMTList.AddToList(pxKnowledge);
			}
		}
	}

}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ProcessMotionTrackerBlips
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::ProcessMotionTrackerBlips( void )
{
	AVP_ServerEntity_Actor* pxServerActor = GetActorKnowledgeSystem()->GetActorBrain()->GetActor();

	for (Asura_Hashed_List_It<AVP_ServerEntity_MotionTrackerBlip> xIt(AVP_ServerEntity_MotionTrackerBlip::GetList()); !xIt.Done(); xIt.Next())
	{
		AVP_ServerEntity_MotionTrackerBlip*	pxMTB = xIt.GetCurrent();		

		// isActor Valid
		if (!pxMTB)
			continue;

		if (!pxMTB->IsEntityActive())
			continue;

		if ( ShouldCullOnRangeAndFOV( pxServerActor, pxMTB->GetPosition() ) )
			continue;

		
		float fVelocity = pxMTB->GetVelocity();

		if ( fVelocity <= m_fDetectableVelocity )
		{
			continue;
		}

		// Now check to see if an entry already exists.
		Axon_Knowledge* pxExistingKnowledge = m_xMTList.FindObject(pxMTB->GetGuid());

		if (pxExistingKnowledge)
		{
			pxExistingKnowledge->m_xDirection		= pxMTB->GetPosition() - pxServerActor->GetPosition();
			pxExistingKnowledge->m_fDistance		= pxExistingKnowledge->m_xDirection.Normalise();
		}
		else
		{
			// Create a new one.
			Axon_Knowledge* pxKnowledge = m_pxKnowledgeSystem->CreateKnowledge();
			if (pxKnowledge)
			{
				GetActorKnowledgeSystem()->ResetAlertTimer();

				pxKnowledge->m_xPosition				= pxMTB->GetPosition();
				pxKnowledge->m_uKnowledgeID				= m_pxKnowledgeSystem->GetNewKnowledgeID();
				pxKnowledge->m_uSourceGuid				= pxMTB->GetGuid();
				pxKnowledge->m_uSourceClassification	= pxMTB->GetClassification();
				pxKnowledge->m_iSenseID					= AVP_SENSE_TYPE_MOTION_TRACKER;
				pxKnowledge->m_uTypeInformation			= AVP_SOUND_TYPE_MOTION_TRACKER_BLIP;
				pxKnowledge->m_fAge						= 0.0f;
				pxKnowledge->m_fThreatValue				= 0.0f;
				pxKnowledge->m_xPosition				= pxMTB->GetPosition();
				pxKnowledge->m_xDirection				= pxMTB->GetPosition() - pxServerActor->GetPosition();
				pxKnowledge->m_fDistance				= pxKnowledge->m_xDirection.Normalise();

				// Add it to the knowledge list.
				m_xMTList.AddToList(pxKnowledge);
			}
		}
	}

}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ShouldCullActorOnRangeAndFOV
//---------------------------------------------------------------------------
bool AVP_Sense_MotionTracker::ShouldCullOnRangeAndFOV(const AVP_ServerEntity_Actor* pxOurActor, const Asura_Vector_3& pxMTBlipPos)
{
	// Cull on range and FOV.
	Asura_Vector_3			xDirection		= pxMTBlipPos - pxOurActor->GetPosition();
	const float				fDistance		= xDirection.Magnitude();

	if ( ShouldCullOnRange( fDistance ) || ShouldCullOnFOV( fDistance, xDirection ) )
	{
		return true;
	}
	else
	{
		return false;
	}
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ShouldCullOnRange
//---------------------------------------------------------------------------
bool AVP_Sense_MotionTracker::ShouldCullOnRange( const float fRange )
{
	return ( fRange > m_fMTRange );
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: ShouldCullOnFOV
//---------------------------------------------------------------------------
bool AVP_Sense_MotionTracker::ShouldCullOnFOV( const float fRange, const Asura_Vector_3& xToEntity )
{
	Asura_Matrix_3x3 xOrientMatrix;
	Asura_Maths::OrientationFromDirection( xOrientMatrix, m_xDirection );
	const Asura_Vector_3 xLocalSpacePoint = xOrientMatrix.ApplyTranspose( xToEntity );
	return !IsPointInFrustum( xLocalSpacePoint );
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: IsPointInFrustum
//---------------------------------------------------------------------------
bool AVP_Sense_MotionTracker::IsPointInFrustum( const Asura_Vector_3& xPoint )
{
	// adapted from Asura_Camera::IsSphereInFrustum

	// xPoint should already be in our local space

	// behind us
	if ( xPoint.z < 0.0f )
	{
		return false;
	}

	// left and right planes
	const float fZX = xPoint.z * m_fTanFovX;
	if ( ( xPoint.x - fZX ) > 0.0f || ( -xPoint.x - fZX ) > 0.0f )
	{
		return false;
	}

	// top and bottom planes
	if ( xPoint.y < 0.0f )
	{
		// top
		const float fZY = xPoint.z * m_fTanFovYUp;
		if ( -xPoint.y - fZY > 0.0f )
		{
			return false;
		}
	}
	else
	{
		// bottom
		const float fZY = xPoint.z * m_fTanFovYDown;
		if ( xPoint.y - fZY > 0.0f )
		{
			return false;
		}
	}

	return true;

}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: GetTargetPriority
//---------------------------------------------------------------------------
int	AVP_Sense_MotionTracker::GetTargetPriority( Asura_Guid uThreatGuid )
{
	if ( uThreatGuid == ASURA_GUID_UNREGISTERED ) { return AVP_AI_TargetPriorityData::IGNORE_P; }

	AVP_ServerEntity_Actor*		pxThreatActor	= AVP_ServerEntity_Actor::FindEntity( uThreatGuid );

	if ( !pxThreatActor ) { return AVP_AI_TargetPriorityData::IGNORE_P; }

	AVP_ServerEntity_Actor*		pxOurActor		= GetActorKnowledgeSystem()->GetActorBrain()->GetActor();
	int							iPriority		= AVP_AI_TargetPriorityData::NORMAL;
	bool						bInValidSquad	= false;
	bool						bUseSquads		= false;
	AVP_ServerEntity_Squad*		pxSquad			= AVP_ServerEntity_Squad::FindEntity( pxOurActor->GetInitialSquadGuid() );

	if ( pxSquad )
	{
		bInValidSquad = true;
	}

	// Actors priority always overrides squads.
	if ( !GetActorKnowledgeSystem()->GetActorBrain()->CalcTargetingPriority( pxThreatActor->GetClassification(), pxThreatActor->GetGuid(), iPriority ) )
	{
		if ( bInValidSquad )
		{
			bUseSquads = true;
		}
	}
	else
	{
		return iPriority;
	}

	if ( bUseSquads )
	{
		int iSquadsPriority = AVP_AI_TargetPriorityData::NORMAL;

		if ( bInValidSquad )
		{
			if ( !pxSquad->CalcTargetingPriority( pxThreatActor->GetClassification(), pxThreatActor->GetGuid(), iSquadsPriority ) )
			{
				return AVP_AI_TargetPriorityData::NORMAL;
			}
			else
			{
				return iSquadsPriority;
			}
		}
	}

	return AVP_AI_TargetPriorityData::NORMAL;
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: GetAVPKnowledgeSystem
//---------------------------------------------------------------------------
AVP_Knowledge_System_Actor* AVP_Sense_MotionTracker::GetActorKnowledgeSystem()
{
	return static_cast< AVP_Knowledge_System_Actor* >( GetKnowledgeSystem() );
}

//---------------------------------------------------------------------------
// AVP_Sense_MotionTracker :: 
//---------------------------------------------------------------------------
void AVP_Sense_MotionTracker::InitFromSettingsBlueprint()
{
	Asura_Blueprint* pxBlueprint = Asura_Blueprint_System::GetBlueprintByHashID( 0x7ad7bd1b /*"AI Settings"*/, 0xebf3ee2c /*"MotionTrackerSense"*/ );

	if ( pxBlueprint )
	{
		Asura_Value xValue;

		if ( pxBlueprint->GetParameterValueByHashID( 0xbe5f176b /*"Motion Tracker Range"*/, xValue ) )
		{
			m_fMTRange = xValue.GetFloatValue();
		}

		if ( pxBlueprint->GetParameterValueByHashID( 0x12dae720 /*"Detectable Velocity"*/, xValue ) )
		{
			m_fDetectableVelocity = xValue.GetFloatValue();
		}
	}

	m_fTanFovX     = Asura_Maths::Tangent( Asura_Maths::DegToRad( 179.0f ) * 0.5f );
	m_fTanFovYUp   = Asura_Maths::Tangent( Asura_Maths::DegToRad( 179.0f ) * 0.25f );
	m_fTanFovYDown = Asura_Maths::Tangent( Asura_Maths::DegToRad( 179.0f ) * 0.25f );
}