#include "Mix/Private/Scene/Common/RootMotionCurve.h"
#include "Mix/IO/IReader.h"

namespace Mix{ namespace Scene{ namespace Common{

const wchar_t* RootMotionCurve::FAILED_CREATE = L"ルートモーションカーブの作成に失敗";

RootMotionCurve* RootMotionCurve::CreateInstance( void )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_SCENE, RootMotionCurve );
}

RootMotionCurve* RootMotionCurve::CreateInstance( Mix::IO::IReader* pReader, const wchar_t* pNameLabel, const wchar_t* pName )
{
	RootMotionCurve* pInterface = NULL;
	UInt32 readSize;
	RootMotionCurve::MOT_CURV_DESC desc;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ヘッダ
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( pReader->Read( &desc, sizeof( desc ) ) != sizeof( desc ) )
	{
		MIX_LOG_ERROR( L"%s : %s(0) : %s[%s]", RootMotionCurve::FAILED_CREATE, Mix::STR_ILLEGALFORMAT, pNameLabel, pName );
		return NULL;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// インターフェースの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pInterface = MIX_LIB_NEW_T( Mix::Memory::SECTION_SCENE, RootMotionCurve );
	if( pInterface == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : %s[%s]", RootMotionCurve::FAILED_CREATE, Mix::STR_OUTOFMEMORY, pNameLabel, pName );
		return NULL;
	}

	//ローテーション
	if( desc.rotationKeyNum > 0 )
	{
		pInterface->m_RotationKeyList.resize( desc.rotationKeyNum );
		MIX_ASSERT( pInterface->m_RotationKeyList.size() == desc.rotationKeyNum );

		readSize = MIX_UIT_TO_UI32( sizeof( Mix::Scene::Common::MOTION_QUATERNION_KEY ) * pInterface->m_RotationKeyList.size() );
		if( pReader->Read( &( pInterface->m_RotationKeyList[0] ), readSize ) != readSize )
		{
			MIX_LOG_ERROR( L"%s : %s(0) : %s[%s]", RootMotionCurve::FAILED_CREATE, Mix::STR_ILLEGALFORMAT, pNameLabel, pName );
			return NULL;
		}
	}

	//トランスレーション
	if( desc.translationKeyNum > 0 )
	{
		pInterface->m_TranslationKeyList.resize( desc.translationKeyNum );
		MIX_ASSERT( pInterface->m_TranslationKeyList.size() == desc.translationKeyNum );

		readSize = MIX_UIT_TO_UI32( sizeof( Mix::Scene::Common::MOTION_VECTOR_KEY ) * pInterface->m_TranslationKeyList.size() );
		if( pReader->Read( &( pInterface->m_TranslationKeyList[0] ), readSize ) != readSize )
		{
			MIX_LOG_ERROR( L"%s : %s(2) : %s[%s]", RootMotionCurve::FAILED_CREATE, Mix::STR_ILLEGALFORMAT, pNameLabel, pName );
			return NULL;
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////

	return pInterface;
}

RootMotionCurve::RootMotionCurve( void )
{
}

RootMotionCurve::~RootMotionCurve( void )
{
}

Boolean RootMotionCurve::IsAvailable( void ) const
{
	if( ( m_RotationKeyList.size() > 0 ) ||
		( m_TranslationKeyList.size() > 0 ) )
	{
		return True;
	}

	return False;
}

void RootMotionCurve::Execute(	Float32 blendRatio,
								Float32 preFrame,
								Float32 frame,
								Mix::ContainerT<Mix::Scene::Common::MOTION_ROOT_BLEND, Mix::Memory::SECTION_SCENE>& container )
{
	if( IsAvailable() == True )
	{
		MOTION_ROOT_BLEND* pBlend = container.Add();

		pBlend->ratio = blendRatio;
		pBlend->rotation = GetRotation( preFrame, frame );
		pBlend->translation = GetTranslation( preFrame, frame );
	}
}

Mix::Quaternion RootMotionCurve::GetRotation( float time )
{
	if( m_RotationKeyList.size() == 0 )
	{
		return Mix::Quaternion::Identity();
	}

	const Mix::Scene::Common::MOTION_QUATERNION_KEY& firstKey = m_RotationKeyList.front();
	const Mix::Scene::Common::MOTION_QUATERNION_KEY& lastKey = m_RotationKeyList.back();

	if( firstKey.time >= time )
	{
		return firstKey.value;
	}
	else if( lastKey.time <= time )
	{
		return lastKey.value;
	}

	Mix::Scene::Common::MotionQuaternionKeyList::iterator it = std::upper_bound( m_RotationKeyList.begin(), m_RotationKeyList.end(), time, Mix::Scene::Common::MOTION_QUATERNION_KEY() );
	if( ( *it ).time == time )
	{
		return ( *it ).value;
	}

	const Mix::Scene::Common::MOTION_QUATERNION_KEY& prevKey = ( *( it - 1 ) );
	const Mix::Scene::Common::MOTION_QUATERNION_KEY& nextKey = ( *it );

	Float32 rate = MIX_FLOAT_DIV( ( time - prevKey.time ), ( nextKey.time - prevKey.time ) );

	return Mix::Quaternion::Slerp( prevKey.value, nextKey.value, rate );
}

Mix::Vector3 RootMotionCurve::GetTranslation( float time )
{
	if( m_TranslationKeyList.size() == 0 )
	{
		return Mix::Vector3::Zero();
	}

	const Mix::Scene::Common::MOTION_VECTOR_KEY& firstKey = m_TranslationKeyList.front();
	const Mix::Scene::Common::MOTION_VECTOR_KEY& lastKey = m_TranslationKeyList.back();

	if( firstKey.time >= time )
	{
		return firstKey.value;
	}
	else if( lastKey.time <= time )
	{
		return lastKey.value;
	}

	Mix::Scene::Common::MotionVectorKeyList::iterator it = std::upper_bound( m_TranslationKeyList.begin(), m_TranslationKeyList.end(), time, Mix::Scene::Common::MOTION_VECTOR_KEY() );
	if( ( *it ).time == time )
	{
		return ( *it ).value;
	}

	const Mix::Scene::Common::MOTION_VECTOR_KEY& prevKey = ( *( it - 1 ) );
	const Mix::Scene::Common::MOTION_VECTOR_KEY& nextKey = ( *it );
	const Mix::Vector3& prevValue = prevKey.value;
	const Mix::Vector3& nextValue = nextKey.value;

	Float32 rate = MIX_FLOAT_DIV( ( time - prevKey.time ), ( nextKey.time - prevKey.time ) );

	return prevValue + ( ( nextValue - prevValue ) * rate );
}

Mix::Quaternion RootMotionCurve::GetRotation( float preFrame, float nextFrame )
{
	if( m_RotationKeyList.size() == 0 )
	{
		return Mix::Quaternion::Identity();
	}

	Mix::Quaternion ret;

	if( preFrame == nextFrame )
	{
		ret = Mix::Quaternion::Identity();
	}
	else if( preFrame < nextFrame )
	{
		const Mix::Scene::Common::MOTION_QUATERNION_KEY& firstKey = m_RotationKeyList[0];

		if( firstKey.time <= preFrame )
		{
			Mix::Quaternion preQ = GetRotation( preFrame );
			Mix::Quaternion nextQ = GetRotation( nextFrame );
		
			ret = nextQ * preQ.ToInverse();
		}
		else
		{
			ret = GetRotation( nextFrame );
		}
	}
	else
	{
		const Mix::Scene::Common::MOTION_QUATERNION_KEY& firstKey = m_RotationKeyList.front();
		const Mix::Scene::Common::MOTION_QUATERNION_KEY& lastKey = m_RotationKeyList.back();

		Mix::Quaternion preQ = GetRotation( preFrame );
		Mix::Quaternion nextQ = GetRotation( nextFrame );

		ret = ( lastKey.value * preQ.ToInverse() ) * ( nextQ * firstKey.value.ToInverse() );
	}

	return ret;
}

Mix::Vector3 RootMotionCurve::GetTranslation( float preFrame, float nextFrame )
{
	if( m_TranslationKeyList.size() == 0 )
	{
		return Mix::Vector3::Zero();
	}

	Mix::Vector3 ret;

	if( preFrame == nextFrame )
	{
		ret = Mix::Vector3::Zero();
	}
	else if( preFrame < nextFrame )
	{
		const Mix::Scene::Common::MOTION_VECTOR_KEY& fristKey = m_TranslationKeyList[0];

		if( fristKey.time <= preFrame )
		{
			Mix::Vector3 preTranslation = GetTranslation( preFrame );
			Mix::Vector3 nextTranslation = GetTranslation( nextFrame );

			ret = nextTranslation - preTranslation;
		}
		else
		{
			ret = GetTranslation( nextFrame );
		}
	}
	else
	{
		const Mix::Scene::Common::MOTION_VECTOR_KEY& firstKey = m_TranslationKeyList.front();
		const Mix::Scene::Common::MOTION_VECTOR_KEY& lastKey = m_TranslationKeyList.back();

		Mix::Vector3 preV = GetTranslation( preFrame );
		Mix::Vector3 nextV = GetTranslation( nextFrame );

		ret = ( lastKey.value - preV ) + ( nextV - firstKey.value );
	}

	return ret;
}

}}}
