
///////////////////////////////////////////////////////////////////////////////
//
// File   : AVP_BoneDataModifier_TargetRelative.cpp
// Author : Will Plano
// Date   : 08/01/2009
//
// Copyright (C) 2009 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

#include "Animation/AVP_BoneDataModifier_TargetRelative.h"

#include "Animation/Asura_Animation.h"
//#include "Debugging/Asura_DebugLineRender.h"

#define AVP_BONEDATAMODIFIER_TARGETRELATIVE_PRIORITY_UPDATETIMERS	0x00001000u
#define AVP_BONEDATAMODIFIER_TARGETRELATIVE_PRIORITY_UPDATEBONEDATA	0x00001000u

#define SCRATCH_SIZE 15
static Asura_Vector_3 s_axOldOSBonePos[ SCRATCH_SIZE ];

AVP_BoneDataModifier_TargetRelative::AVP_BoneDataModifier_TargetRelative( Asura_Animation* pxAnimation, const Asura_Vector_3& xRelativeOSTargetPosition, const u_int uNumBones, const int* const aiBone, const bool bRescale )
: PARENT( pxAnimation, AVP_BONEDATAMODIFIER_TARGETRELATIVE_PRIORITY_UPDATETIMERS, AVP_BONEDATAMODIFIER_TARGETRELATIVE_PRIORITY_UPDATEBONEDATA )
, m_xRelativeOSTargetPosition( xRelativeOSTargetPosition )
, m_xCurrentOSTargetPosition( xRelativeOSTargetPosition )
, m_axRestPoseOSBonePos( NULL )
, m_aiBone( aiBone )
, m_uNumBones( uNumBones )
, m_uNumTimerUpdatesAtLastBoneUpdate( 0 )
, m_fBlend( 0.0f )
, m_fInvTweenTime( 0.0f )
, m_uFlags( 0 )
{
    m_bRescale = bRescale;

	Asura_Assert(m_pxAnimation && m_pxAnimation->GetSkin(), "UpdateBoneData() failed - NULL Skin pointer");
	if (!m_pxAnimation || !m_pxAnimation->GetSkin()) 
	{ 
		return; 
	}

	m_axRestPoseOSBonePos = new Asura_Vector_3[ uNumBones ];
	for ( u_int uItr = 0; uItr < uNumBones; ++uItr )
	{
		m_axRestPoseOSBonePos[ uItr ].Zero();
		int iBone = aiBone[ uItr ];
		Asura_Assert( iBone < m_pxAnimation->GetSkin()->NumberOfBones, "Not a valid bone" );
		if ( iBone < m_pxAnimation->GetSkin()->NumberOfBones )
		{
			do
			{
				m_axRestPoseOSBonePos[ uItr ] += pxAnimation->GetSkin()->BoneRestPose[ iBone ].Position;
				m_axRestPoseOSBonePos[ uItr ] = pxAnimation->GetSkin()->BoneRestPose[ iBone ].Quat.RotateVector( m_axRestPoseOSBonePos[ uItr ] );
				iBone = pxAnimation->GetSkin()->GetParentBoneIndex( iBone );
			} while ( iBone > 0 );
		}
	}
}

AVP_BoneDataModifier_TargetRelative::~AVP_BoneDataModifier_TargetRelative()
{
	delete[] m_axRestPoseOSBonePos;
}

void AVP_BoneDataModifier_TargetRelative::TweenIn( const float fTweenTime )
{
	m_bTweenIn = true;
	m_fInvTweenTime = fTweenTime == 0.0f ? Asura_Maths::LargeFloat : 1.0f / fTweenTime;
}

void AVP_BoneDataModifier_TargetRelative::TweenOut( const float fTweenTime )
{
	m_bTweenIn = false;
	m_fInvTweenTime = fTweenTime == 0.0f ? Asura_Maths::LargeFloat : 1.0f / fTweenTime;
}

void AVP_BoneDataModifier_TargetRelative::UpdateTimers( const float fTime )
{
	float fDelta = fTime * m_fInvTweenTime;
	if ( m_bTweenIn )
	{
		fDelta *= -1.0f;
	}
	m_fBlend = Asura_Maths::ClampToRange( m_fBlend - fDelta , 0.0f, 1.0f ); 
}

void AVP_BoneDataModifier_TargetRelative::UpdateBoneData( const int iFirstBone, const int iLastBone )
{
	Asura_Assert(m_pxAnimation && m_pxAnimation->GetSkin(), "UpdateBoneData() failed - NULL Skin pointer");
	if (!m_pxAnimation || !m_pxAnimation->GetSkin()) 
	{ 
		return; 
	}
	Asura_Assert(m_uNumBones < SCRATCH_SIZE, "Too many bones in AVP_BoneDataModifier_TargetRelative");
	if (m_uNumBones >= SCRATCH_SIZE) 
	{ 
		return; 
	}
	Asura_Assert( iLastBone < m_pxAnimation->GetSkin()->NumberOfBones, "" );
	if( m_uNumTimerUpdatesAtLastBoneUpdate != m_pxAnimation->GetNumTimerUpdates() )
	{
		m_uNumTimerUpdatesAtLastBoneUpdate = m_pxAnimation->GetNumTimerUpdates();
		if ( m_fBlend > 0.0f )
		{
			for ( u_int uItr = 0; uItr < m_uNumBones; ++uItr )
			{
				//Asura_DebugLineRender::AddCross( m_pxAnimation->GetOrientQuat().RotateVector( m_axRestPoseOSBonePos[ uItr ] ) + m_pxAnimation->GetPosition(), 0x8000FF00, 0.15f );
				m_pxAnimation->GetBoneLocation( m_aiBone[uItr], &s_axOldOSBonePos[uItr], NULL, false );
				//Asura_DebugLineRender::AddCross( m_pxAnimation->GetOrientQuat().RotateVector( s_axOldOSBonePos[uItr] ) + m_pxAnimation->GetPosition(), 0x80FF0000, 0.15f );
			}

			for ( int iBoneItr = iFirstBone; iBoneItr <= iLastBone; ++iBoneItr )
			{
				int iParentBone = m_pxAnimation->GetSkin()->GetParentBoneIndex( iBoneItr );
				bool bIsBoneValid = false;
				bool bIsParentBoneValid = false;
				u_int uValidBoneItr = 0;
				for ( u_int uItr = 0; !bIsBoneValid && uItr < m_uNumBones; ++uItr )
				{
					if ( m_aiBone[ uItr ] == iBoneItr )
					{
						bIsBoneValid = true;
						uValidBoneItr = uItr;
					}
					if ( m_aiBone[ uItr ] == iParentBone )
					{
						bIsParentBoneValid = true;
					}
				}

				if ( bIsBoneValid )
				{
					// get bone pos/ori in relative target space ( where the z axis is mapped onto the line from the rest position of the bone to the relative target position )
					Asura_Vector_3 xRestBonePosToRelativeTarget = m_xRelativeOSTargetPosition - m_axRestPoseOSBonePos[ uValidBoneItr ];
					Asura_Matrix_3x3 axInvRelativeTargetSpaceTransformMatrix(	1.0f, 0.0f, -xRestBonePosToRelativeTarget.GetX() / xRestBonePosToRelativeTarget.GetZ(),
																				0.0f, 1.0f, -xRestBonePosToRelativeTarget.GetY() / xRestBonePosToRelativeTarget.GetZ(),
																				0.0f, 0.0f, 1.0f / xRestBonePosToRelativeTarget.GetZ() );

					Asura_Vector_3 xRTSBonePos = ( s_axOldOSBonePos[ uValidBoneItr ] - m_axRestPoseOSBonePos[ uValidBoneItr ] ) * axInvRelativeTargetSpaceTransformMatrix;

					// then back to object space using the current target space transform
					Asura_Vector_3 xRestBonePosToCurrentTarget = m_xCurrentOSTargetPosition - m_axRestPoseOSBonePos[ uValidBoneItr ];
					Asura_Matrix_3x3 axCurrentTargetSpaceTransformMatrix(	1.0f, 0.0f, xRestBonePosToCurrentTarget.GetX(),
																			0.0f, 1.0f, xRestBonePosToCurrentTarget.GetY(),
																			0.0f, 0.0f, xRestBonePosToCurrentTarget.GetZ() );

					Asura_Vector_3 xNewOSBonePos = ( xRTSBonePos * axCurrentTargetSpaceTransformMatrix ) + m_axRestPoseOSBonePos[ uValidBoneItr ];
					//Asura_DebugLineRender::AddCross( m_pxAnimation->GetOrientQuat().RotateVector( xNewOSBonePos ) + m_pxAnimation->GetPosition(), 0x800000FF, 0.15f );
					
					// and finally put back into tween store
					Asura_Vector_3 xNewOSParentBonePos = Asura_Maths::ZeroVector3;
					Asura_Matrix_3x3 xNewOSParentBoneOri = Asura_Maths::IdentityMatrix3x3;;
					if ( iParentBone >= 0 )
					{
						m_pxAnimation->GetBoneLocation( iParentBone, &xNewOSParentBonePos, &xNewOSParentBoneOri, false );
					}

					Asura_Vector_3 xNewPos = xNewOSParentBoneOri.ApplyTranspose( xNewOSBonePos - xNewOSParentBonePos );

					Asura_Vector_3 xOldPos;
					m_pxAnimation->GetTweenStore()->GetBoneData( iBoneItr ).GetPosition( xOldPos );
					xNewPos.Lerp( xOldPos, xNewPos, m_fBlend );
					xOldPos.Normalise();
					float fNewLen = xNewPos.Normalise();
					if ( iParentBone > 0 && bIsParentBoneValid )
					{
						Asura_Quat xOldQuat;
						m_pxAnimation->GetTweenStore()->GetBoneData( iParentBone ).GetOrientation( xOldQuat );

						Asura_Quat xNewQuat;
						xNewQuat.RotationFromAToB( xOldPos, xNewPos );
						m_pxAnimation->GetTweenStore()->GetBoneData( iParentBone ).SetOrientation( xNewQuat * xOldQuat );
					}

					if ( m_bRescale )
					{
						xNewPos = xOldPos * fNewLen;
						m_pxAnimation->GetTweenStore()->GetBoneData( iBoneItr ).SetPosition( xNewPos );
					}
				}
			}
		}
	}
}
