#include "Mix/Tool/Win32/Core/Graphics/RootMotionCurve.h"

#include <algorithm>
#include "Mix/Tool/Win32/Core/Graphics/ObjectModel.h"

namespace Mix{ namespace Tool{ namespace Win32{ namespace Graphics{

RootMotionCurve::RootMotionCurve( void )
{
}

RootMotionCurve::~RootMotionCurve( void )
{
}

void RootMotionCurve::Initialize( unsigned int rKeyCount,  unsigned int tKeyCount )
{
	Destroy();

	m_RotationKeyList.reserve( rKeyCount );
	m_TranslationKeyList.reserve( tKeyCount );
}

void RootMotionCurve::Destroy( void )
{
	m_RotationKeyList.clear();
	m_TranslationKeyList.clear();
}

void RootMotionCurve::AddRotationKey( float time, const D3DXQUATERNION& value )
{
	D3DXQUATERNION normValue;

	D3DXQuaternionNormalize( &normValue, &value );
	m_RotationKeyList.push_back( Mix::Tool::Win32::Graphics::QUATERNION_KEY( time, normValue ) );
}

unsigned int RootMotionCurve::GetRotationKeyCount( void ) const
{
	return m_RotationKeyList.size();
}

const Mix::Tool::Win32::Graphics::QUATERNION_KEY& RootMotionCurve::GetRotationKey( int index ) const
{
	return m_RotationKeyList[index];
}

void RootMotionCurve::AddTranslationKey( float time, const D3DXVECTOR3& value )
{
	m_TranslationKeyList.push_back( Mix::Tool::Win32::Graphics::VECTOR_KEY( time, value ) );
}

unsigned int RootMotionCurve::GetTranslationKeyCount( void ) const
{
	return m_TranslationKeyList.size();
}

const Mix::Tool::Win32::Graphics::VECTOR_KEY& RootMotionCurve::GetTranslationKey( int index ) const
{
	return m_TranslationKeyList[index];
}

float RootMotionCurve::GetLastTime( void )
{
	float lastTime = 0;

	for( Mix::Tool::Win32::Graphics::QuaternionKeyList::iterator it = m_RotationKeyList.begin(); it != m_RotationKeyList.end(); ++it )
	{
		if( lastTime < ( *it ).time ) { lastTime = ( *it ).time; }
	}

	for( Mix::Tool::Win32::Graphics::VectorKeyList::iterator it = m_TranslationKeyList.begin(); it != m_TranslationKeyList.end(); ++it )
	{
		if( lastTime < ( *it ).time ) { lastTime = ( *it ).time; }
	}

	return lastTime;
}

bool RootMotionCurve::ExistsKey( void ) const
{
	return ( ( m_RotationKeyList.size() > 0 ) || ( m_TranslationKeyList.size() > 0 ) );
}

D3DXQUATERNION RootMotionCurve::GetRotation( float time )
{
	if( m_RotationKeyList.size() == 0 )
	{
		return D3DXQUATERNION( 0.0f, 0.0f, 0.0f, 1.0f );
	}

	const Mix::Tool::Win32::Graphics::QUATERNION_KEY& firstKey = m_RotationKeyList.front();
	const Mix::Tool::Win32::Graphics::QUATERNION_KEY& lastKey = m_RotationKeyList.back();

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

	Mix::Tool::Win32::Graphics::QuaternionKeyList::iterator it = std::upper_bound( m_RotationKeyList.begin(), m_RotationKeyList.end(), time, Mix::Tool::Win32::Graphics::QUATERNION_KEY() );
	if( ( *it ).time == time )
	{
		return ( *it ).value;
	}

	const Mix::Tool::Win32::Graphics::QUATERNION_KEY& prevKey = ( *( it - 1 ) );
	const Mix::Tool::Win32::Graphics::QUATERNION_KEY& nextKey = ( *it );

	float ratio = MIX_FLOAT_DIV( ( time - prevKey.time ), ( nextKey.time - prevKey.time ) );

	D3DXQUATERNION temp;

	return *D3DXQuaternionSlerp( &temp, &( prevKey.value ), &( nextKey.value ), ratio );
}

D3DXVECTOR3 RootMotionCurve::GetTranslation( float time )
{
	if( m_TranslationKeyList.size() == 0 )
	{
		return D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	}

	const Mix::Tool::Win32::Graphics::VECTOR_KEY& firstKey = m_TranslationKeyList.front();
	const Mix::Tool::Win32::Graphics::VECTOR_KEY& lastKey = m_TranslationKeyList.back();

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

	Mix::Tool::Win32::Graphics::VectorKeyList::iterator it = std::upper_bound( m_TranslationKeyList.begin(), m_TranslationKeyList.end(), time, Mix::Tool::Win32::Graphics::VECTOR_KEY() );
	if( ( *it ).time == time )
	{
		return ( *it ).value;
	}

	const Mix::Tool::Win32::Graphics::VECTOR_KEY& prevKey = ( *( it - 1 ) );
	const Mix::Tool::Win32::Graphics::VECTOR_KEY& nextKey = ( *it );

	float ratio = MIX_FLOAT_DIV( ( time - prevKey.time ), ( nextKey.time - prevKey.time ) );

	return prevKey.value + ( ( nextKey.value - prevKey.value ) * ratio );
}

D3DXQUATERNION RootMotionCurve::GetRotation( float preTime, float nextTime )
{
	if( m_RotationKeyList.size() == 0 )
	{
		return D3DXQUATERNION( 0.0f, 0.0f, 0.0f, 1.0f );
	}

	const Mix::Tool::Win32::Graphics::QUATERNION_KEY& firstKey = m_RotationKeyList.front();

	D3DXQUATERNION ret;

	if( firstKey.time > preTime )
	{
		ret = firstKey.value;
	}
	else if( preTime == nextTime )
	{
		ret = D3DXQUATERNION( 0.0f, 0.0f, 0.0f, 1.0f );
	}
	else if( preTime < nextTime )
	{
		D3DXQUATERNION preQ = GetRotation( preTime );
		D3DXQUATERNION nextQ = GetRotation( nextTime );

		D3DXQUATERNION temp;

		D3DXQuaternionInverse( &temp, &preQ );
		D3DXQuaternionMultiply( &ret, &nextQ, &temp );
	}
	else
	{
		const D3DXQUATERNION& firstQ = GetRotationKey( 0 ).value;
		const D3DXQUATERNION& lastQ = GetRotationKey( GetRotationKeyCount() - 1 ).value;

		D3DXQUATERNION preQ = GetRotation( preTime );
		D3DXQUATERNION nextQ = GetRotation( nextTime );

		D3DXQUATERNION temp;
		D3DXQUATERNION q0;
		D3DXQUATERNION q1;

		D3DXQuaternionInverse( &temp, &preQ );
		D3DXQuaternionMultiply( &q0, &lastQ, &temp );

		D3DXQuaternionInverse( &temp, &firstQ );
		D3DXQuaternionMultiply( &q1, &nextQ, &temp );

		D3DXQuaternionMultiply( &ret, &q0, &q1 );
	}

	return ret;
}

D3DXVECTOR3 RootMotionCurve::GetTranslation( float preTime, float nextTime )
{
	if( m_TranslationKeyList.size() == 0 )
	{
		return D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	}

	D3DXVECTOR3 ret;

	const Mix::Tool::Win32::Graphics::VECTOR_KEY& firstKey = m_TranslationKeyList.front();

	if( firstKey.time > preTime )
	{
		ret = firstKey.value;
	}
	else if( preTime == nextTime )
	{
		ret = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	}
	else if( preTime < nextTime )
	{
		D3DXVECTOR3 preTranslation = GetTranslation( preTime );
		D3DXVECTOR3 nextTranslation = GetTranslation( nextTime );

		D3DXVec3Subtract( &ret, &nextTranslation, &preTranslation );
	}
	else
	{
		const D3DXVECTOR3& firstTranslation = GetTranslationKey( 0 ).value;
		const D3DXVECTOR3& lastTranslation = GetTranslationKey( GetTranslationKeyCount() - 1 ).value;

		D3DXVECTOR3 preTranslation = GetTranslation( preTime );
		D3DXVECTOR3 nextTranslation = GetTranslation( nextTime );

		D3DXVECTOR3 temp;

		D3DXVec3Subtract( &ret, &lastTranslation, &preTranslation );
		D3DXVec3Subtract( &temp, &nextTranslation, &firstTranslation);
		D3DXVec3Add( &ret, &ret, &temp );
	}

	return ret;
}
/*
bool RootMotionCurve::Read( Mix::Tool::Win32::File::InputStream& input, unsigned int rKeyCount, unsigned int tKeyCount )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 解放
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_RotationKeyList.clear();
	m_DirectionKeyList.clear();
	m_TranslationKeyList.clear();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ローテーションキーリスト
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( rKeyCount > 0 )
	{
		std::vector<RootMotionCurve::MC_R_KEY> readList;
		std::vector<RootMotionCurve::MC_R_KEY>::iterator it_begin;
		std::vector<RootMotionCurve::MC_R_KEY>::iterator it_end;
		std::vector<RootMotionCurve::MC_R_KEY>::iterator it;

		readList.resize( rKeyCount );
		it_begin = readList.begin();
		it_end = readList.end();

		if( input.Read( &( readList[0] ), sizeof( RootMotionCurve::MC_R_KEY ) * rKeyCount ) == false )
		{
			return false;
		}

		m_RotationKeyList.reserve( rKeyCount );

		for( it = it_begin; it != it_end; ++it )
		{
			const RootMotionCurve::MC_R_KEY* pSrc = &( *it );
			Mix::Tool::Win32::Graphics::ROOT_ROTATION_KEY dst;

			dst.time = pSrc->time;
			dst.value.x = pSrc->value[0];
			dst.value.y = pSrc->value[1];
			dst.value.z = pSrc->value[2];
			dst.value.w = pSrc->value[3];

			m_RotationKeyList.push_back( dst );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// トランスレーションキーリスト
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( tKeyCount > 0 )
	{
		std::vector<RootMotionCurve::MC_T_KEY> readList;
		std::vector<RootMotionCurve::MC_T_KEY>::iterator it_begin;
		std::vector<RootMotionCurve::MC_T_KEY>::iterator it_end;
		std::vector<RootMotionCurve::MC_T_KEY>::iterator it;

		readList.resize( tKeyCount );
		it_begin = readList.begin();
		it_end = readList.end();

		if( input.Read( &( readList[0] ), sizeof( RootMotionCurve::MC_T_KEY ) * tKeyCount ) == false )
		{
			return false;
		}

		m_TranslationKeyList.reserve( tKeyCount );

		for( it = it_begin; it != it_end; ++it )
		{
			const RootMotionCurve::MC_T_KEY* pSrc = &( *it );
			Mix::Tool::Win32::Graphics::TRANSLATION_KEY dst;

			dst.time = pSrc->time;
			dst.value.x = pSrc->value[0];
			dst.value.y = pSrc->value[1];
			dst.value.z = pSrc->value[2];

			m_TranslationKeyList.push_back( dst );
		}
	}

	return true;
}
*/
bool RootMotionCurve::Write( Mix::Tool::Win32::File::OutputStream& output )
{
	if( ( m_RotationKeyList.size() == 0 ) &&
		( m_TranslationKeyList.size() == 0 ) )
	{
		return false;
	}

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

	{
		RootMotionCurve::MOT_RT_DESC desc;

		desc.rotationKeyNum = m_RotationKeyList.size();
		desc.translationKeyNum = m_TranslationKeyList.size();

		if( output.Write( &desc, sizeof( desc ) ) == false )
		{
			return false;
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ローテーション
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_RotationKeyList.size() > 0 )
	{
		unsigned int writeSize = sizeof( RootMotionCurve::MOT_Q_KEY ) * m_RotationKeyList.size();
		std::vector<RootMotionCurve::MOT_Q_KEY> keyList;

		keyList.reserve( m_RotationKeyList.size() );

		for( Mix::Tool::Win32::Graphics::QuaternionKeyList::iterator it = m_RotationKeyList.begin(); it != m_RotationKeyList.end(); ++it )
		{
			const Mix::Tool::Win32::Graphics::QUATERNION_KEY& srcKey = ( *it );
			MOT_Q_KEY key;

			key.time = static_cast<float>( srcKey.time );
			key.reserve = 0;
			key.value[0] = srcKey.value.x;
			key.value[1] = srcKey.value.y;
			key.value[2] = srcKey.value.z;
			key.value[3] = srcKey.value.w;

			keyList.push_back( key );
		}

		if( output.Write( &( keyList[0] ), writeSize ) == false )
		{
			return false;
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// トランスレーション
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_TranslationKeyList.size() > 0 )
	{
		unsigned int writeSize = sizeof( RootMotionCurve::MOT_V_KEY ) * m_TranslationKeyList.size();
		std::vector<RootMotionCurve::MOT_V_KEY> keyList;

		keyList.reserve( m_TranslationKeyList.size() );

		for( Mix::Tool::Win32::Graphics::VectorKeyList::iterator it = m_TranslationKeyList.begin(); it != m_TranslationKeyList.end(); ++it )
		{
			const Mix::Tool::Win32::Graphics::VECTOR_KEY& srcKey = ( *it );
			MOT_V_KEY key;

			key.time = static_cast<float>( srcKey.time );
			key.value[0] = srcKey.value.x;
			key.value[1] = srcKey.value.y;
			key.value[2] = srcKey.value.z;

			keyList.push_back( key );
		}

		if( output.Write( &( keyList[0] ), writeSize ) == false )
		{
			return false;
		}
	}

	return true;
}

Mix::Tool::Win32::Object::TYPE RootMotionCurve::GetType( void ) const
{
	return Mix::Tool::Win32::Object::GRAPHICS__ROOT_MOTION_CURVE;
}

}}}}
