
///////////////////////////////////////////////////////////////////////////////
//
// File   : Asura_Animation_IK_Chain_LOS.cpp
// Author : Will Plano
// Date   : 24/08/2007
//
// Copyright (C) 2007 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

#ifdef ASURA_USE_ANIMATION_IK_SOLVER

#include "Animation/IK/Asura_Animation_IK_Chain_LOS.h"

#include "Animation/IK/Asura_Animation_IK_Link.h"
#include "Core/Asura_Maths.h"
#include "Core/Asura_LOS.h"
#include "Core/Asura_Timers.h"
#include "Debugging/Asura_DebugLineRender.h"
#include "Entity/Asura_Entity_Renderable.h"
#include "Entity/Asura_Entity_System.h"
#include "Interface/Asura_CommandConsole.h"
#include "Mesh/Asura_Mesh_PolygonFlags.h"

DVSVAR bool  g_bRenderDebugInfo = false;

#if defined(ASURA_SUPPORT_COMMAND_CONSOLE) && defined(ASURA_DEBUG_VARIABLE_SYSTEM)
void Asura_Animation_IK_Chain_LOS::AddConsoleVars()
{
#if !defined(ASURA_NO_DEBUG_OUTPUT)
	Asura_CommandConsole::AddVar( L"IK.RenderLOSDebugInfo", &g_bRenderDebugInfo, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
#endif
}
#endif

//////////////////////////////////////////////////////////////////////////
// class Asura_Animation_IK_Chain_LOS
//////////////////////////////////////////////////////////////////////////

Asura_Animation_IK_Chain_LOS::Asura_Animation_IK_Chain_LOS( const u_int uNumBones /* = 0 */, const Asura_AVector3& xLocalLOSDirection /* = Asura_Maths::ZeroAVector3 */ )
: PARENT( uNumBones )
, m_xExclusionFunctor( m_xLOSOutputData )
, m_xLOSDirection( xLocalLOSDirection )
, m_bTestAgainstEverything( false )
, m_bLosDirectionInObjectSpace( true )
, m_xLOSInputData( Asura_Maths::ZeroVector3, Asura_Maths::ZeroVector3, ASURA_GUID_UNREGISTERED, 0.0f, false, AsuraPolygonFlag_IgnoreForIK )
, m_uLOSDataFrameStamp( 0 )
{
	m_xLOSOutputData.Reset( Asura_Maths::LargeFloat );
}

Asura_Animation_IK_Chain_LOS::Asura_Animation_IK_Chain_LOS( const int* piBoneIDs, const int* piTwistBoneIDs /* = NULL */, const Asura_AVector3& xLocalLOSDirection /* = Asura_Maths::ZeroAVector3 */ )
: PARENT( piBoneIDs, piTwistBoneIDs )
, m_xExclusionFunctor( m_xLOSOutputData )
, m_xLOSDirection( xLocalLOSDirection )
, m_bTestAgainstEverything( false )
, m_bLosDirectionInObjectSpace( true )
, m_xLOSInputData( Asura_Maths::ZeroVector3, Asura_Maths::ZeroVector3, ASURA_GUID_UNREGISTERED, 0.0f, false, AsuraPolygonFlag_IgnoreForIK )
, m_uLOSDataFrameStamp( 0 )
{
	m_xLOSOutputData.Reset( Asura_Maths::LargeFloat );
}

Asura_Animation_IK_Chain_LOS::~Asura_Animation_IK_Chain_LOS()
{

}

bool Asura_Animation_IK_Chain_LOS::CalculateTarget()
{
	Asura_AVector3 xEndPos = m_pxLastLink->GetPosition();
	if ( xEndPos.GetX() > 1.0e+020f )
	{
		// End position not yet defined, try anim pos;
		xEndPos = m_pxLastLink->GetAnimPosition();
		if ( xEndPos.GetX() > 1.0e+020f )
		{
			return false;
		}
	}

	// Set up LOS input data
	Asura_AVector3 xWSLOSDirection = m_xLOSDirection;
	if ( m_bLosDirectionInObjectSpace )
	{
		xWSLOSDirection = m_xAnimOrientation.RotateVector( xWSLOSDirection );
	}

	const float fLOSRange = 3.0f + m_fGroundOffset;
	m_xLOSInputData.m_xOrigin = Asura_Vector_3( xEndPos - xWSLOSDirection );
	m_xLOSInputData.m_xDirection = Asura_Vector_3( xWSLOSDirection );
	m_xLOSInputData.m_fInitialMaximumRange = fLOSRange;

	m_xLOSOutputData.Reset( fLOSRange );
	m_xLOSOutputData = *Asura_LOS::TestAgainstEnvironment( &m_xLOSInputData );
	if ( m_bTestAgainstEverything )
	{
		Asura_Entity_System::TestEntitiesWithLOSFunctor( &m_xLOSInputData, m_xExclusionFunctor );
		if (m_xLOSOutputData.m_uHitEntity)
		{
			m_xLOSOutputData.m_bHitEnvironment = false;
		}
	}
	m_uLOSDataFrameStamp = Asura_Timers::GetFrameNumber();

#ifndef ASURA_NO_DEBUG_OUTPUT
	if ( g_bRenderDebugInfo )
	{
		Asura_DebugLineRender::AddLine( xEndPos - xWSLOSDirection						, xEndPos + ( xWSLOSDirection * ( 2.0f + m_fGroundOffset ) ), m_xLOSOutputData.m_bHitEnvironment ? 0xFF00FF00 : ( m_xLOSOutputData.m_uHitEntity ? 0xFF0000FF : 0xFFFF0000 ) );
		Asura_DebugLineRender::AddLine( xEndPos - Asura_AVector3( 0.1f, 0.0f, 0.0f )	, xEndPos + Asura_AVector3( 0.1f, 0.0f, 0.0f )      , 0xFFFFFFFF );
		Asura_DebugLineRender::AddLine( xEndPos - Asura_AVector3( 0.0f, 0.0f, 0.1f )	, xEndPos + Asura_AVector3( 0.0f, 0.0f, 0.1f )      , 0xFFFFFFFF );
	}
#endif
	if ( m_xLOSOutputData.m_bHitEnvironment || m_xLOSOutputData.m_uHitEntity != 0 )
	{
		Asura_AVector3 xWSNormal( m_xLOSOutputData.m_xNormal );
		Asura_Assert( Asura_Maths::Abs( xWSNormal.MagnitudeSquared() - 1.0f ) < 2.0e-4, "Normal not normalised" ); // ~ eps^2 + 2 * eps
		Asura_Assert( Asura_Maths::Abs( xWSLOSDirection.MagnitudeSquared() - 1.0f ) < 2.0e-4, "LOS direction not normalised" );
		Asura_AVector3 xLocalUp = m_xAnimOrientation.RotateVector( Asura_AVector3( 0.0f, -1.0f, 0.0f ));
		m_xTargetOrientationDelta.RotationFromAToB( xLocalUp, xWSNormal);
		Asura_AVector3 xLocalNormal = m_xAnimInvOrientation.RotateVector( xWSNormal);
		float fOffset = m_fGroundOffset / Asura_Maths::Max( 0.5f, -( xLocalNormal.GetY() ) );
		m_xTargetPosition = xEndPos + ( xWSLOSDirection * ( m_xLOSOutputData.m_fDistanceToIntersection - ( 1.0f + fOffset ) ) );
#ifndef ASURA_NO_DEBUG_OUTPUT
		if ( g_bRenderDebugInfo )
		{
			Asura_DebugLineRender::AddLine( m_xTargetPosition - Asura_AVector3( 0.1f, 0.0f, 0.0f ), m_xTargetPosition + Asura_AVector3( 0.1f, 0.0f, 0.0f ), m_xLOSOutputData.m_bHitEnvironment ? 0xFF00FF00 : 0xFF0000FF );
			Asura_DebugLineRender::AddLine( m_xTargetPosition - Asura_AVector3( 0.0f, 0.0f, 0.1f ), m_xTargetPosition + Asura_AVector3( 0.0f, 0.0f, 0.1f ), m_xLOSOutputData.m_bHitEnvironment ? 0xFF00FF00 : 0xFF0000FF );
		}
#endif
		return true;
	}
	else
	{
		return false;
	}
}

bool Asura_Animation_IK_Chain_LOS::ExceptionFunctor::CheckEntity( Asura_Entity_Renderable* pxEntity, const Asura_LOS_InputData& xInputData )
{
	Asura_Assert( pxEntity, "Null leaf object pointer" );
	if ( pxEntity )
	{
		bool bDoTest = true;
		if ( m_ausClassification )
		{
			for ( u_int uClassificationItr = 0; uClassificationItr < m_uNumClassificationExceptions && bDoTest; ++uClassificationItr )
			{
				if ( pxEntity->GetClassification() == m_ausClassification[ uClassificationItr ] )
				{
					bDoTest = false;
				}
			}
		}

		if ( m_auGuid && bDoTest )
		{
			for ( u_int uGuidItr = 0; uGuidItr < m_uNumGuidExceptions && bDoTest; ++uGuidItr )
			{
				if ( pxEntity->GetGuid() == m_auGuid[ uGuidItr ] )
				{
					bDoTest = false;
				}
			}
		}

		if ( bDoTest )
		{
			return pxEntity->LOSTest( &xInputData, &m_rxOutData );
		}
	}
	return false;
}

#endif // ASURA_USE_ANIMATION_IK_SOLVER
