#include "Mix/Private/Scene/Common/DynamicsWorld.h"

#include "Mix/Dynamics/IManager.h"
#include "Mix/Dynamics/IObject.h"
#include "Mix/Dynamics/IObjectDragger.h"
#include "Mix/Dynamics/IStaticMesh.h"
#include "Mix/Scene/IDynamicsObject.h"

#include "Mix/Private/Dynamics/ObjectContext.h"
#include "Mix/Private/Scene/Common/Camera.h"
#include "Mix/Private/Scene/Common/DynamicsObject.h"

namespace Mix{ namespace Scene{ namespace Common{

////////////////////////////////////////////////////////////////////////////////////////////////////
// DynamicsWorld::ContactProcessor
////////////////////////////////////////////////////////////////////////////////////////////////////

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

DynamicsWorld::ContactProcessor::ContactProcessor( void )
{
}

DynamicsWorld::ContactProcessor::~ContactProcessor( void )
{
}

void DynamicsWorld::ContactProcessor::OnAddedToWorld( Mix::Dynamics::IObject* pObject, Mix::Dynamics::IWorld* pWorld )
{
}

void DynamicsWorld::ContactProcessor::OnRemovedFromWorld( Mix::Dynamics::IObject* pObject, Mix::Dynamics::IWorld* pWorld )
{
}

void DynamicsWorld::ContactProcessor::OnContact( Mix::Dynamics::IObject* pObject, const Mix::Dynamics::MANIFOLD& manifold )
{
	MIX_ASSERT( pObject->GetUserPtr() != NULL );
	MIX_ASSERT( manifold.pObjectB->GetUserPtr() != NULL );

	Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER* pObjWrapA = static_cast<Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER*>( pObject->GetUserPtr() );
	Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER* pObjWrapB = static_cast<Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER*>( manifold.pObjectB->GetUserPtr() );

	//リスナーが存在しているときだけ、ContactProcessor( IObjectListener ) がオブジェクトに設定されるので、
	//両方ともリスナーを所持していないのに、このハンドラが呼び出されるのはおかしい。
	MIX_ASSERT( ( pObjWrapA->pInternalObject->HasContactListener() == True ) || ( pObjWrapB->pInternalObject->HasContactListener() == True ) );
/*
	if( ( pObjWrapA->pInternalObject->HasContactListener() == False ) &&
		( pObjWrapB->pInternalObject->HasContactListener() == False ) )
	{
		//両方ともリスナーを所持していなかったら処理しない
		return;
	}
*/
	UInt32 pointCount = manifold.pointCount;
	MIX_ASSERT( manifold.pointCount > 0 );

	//ポイントの要素を二つ分確保
	m_Points.resize( pointCount << 1 );

	const Mix::Dynamics::MANIFOLD_POINT* pSrcPoint = &( manifold.points[0] );
	const Mix::Dynamics::MANIFOLD_POINT* pSrcPointEnd = pSrcPoint + pointCount;

	Mix::Scene::DYNAMICS_CONTACT_POINT* pointsA = &( m_Points[0] );
	Mix::Scene::DYNAMICS_CONTACT_POINT* pointsB = pointsA + pointCount;

	Mix::Scene::DYNAMICS_CONTACT_POINT* pDstPointA = pointsA;
	Mix::Scene::DYNAMICS_CONTACT_POINT* pDstPointB = pointsB;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ポイント配列を作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	while( pSrcPoint != pSrcPointEnd )
	{
		pDstPointA->localPositionA = pSrcPoint->localPositionA;
		pDstPointA->localPositionB = pSrcPoint->localPositionB;
		pDstPointA->worldPositionA = pSrcPoint->worldPositionA;
		pDstPointA->worldPositionB = pSrcPoint->worldPositionB;
		pDstPointA->worldNormalB = pSrcPoint->worldNormalB;
		pDstPointA->penetrationB = pSrcPoint->penetrationB;
		pDstPointA->materialA = pSrcPoint->materialA;
		pDstPointA->materialB = pSrcPoint->materialB;
		pDstPointA->impulse = pSrcPoint->impulse;

		pDstPointB->localPositionA = pSrcPoint->localPositionB;
		pDstPointB->localPositionB = pSrcPoint->localPositionA;
		pDstPointB->worldPositionA = pSrcPoint->worldPositionB;
		pDstPointB->worldPositionB = pSrcPoint->worldPositionA;
		pDstPointB->worldNormalB = -pSrcPoint->worldNormalB;
		pDstPointB->penetrationB = pSrcPoint->penetrationB;
		pDstPointB->materialA = pSrcPoint->materialB;
		pDstPointB->materialB = pSrcPoint->materialA;
		pDstPointB->impulse = pSrcPoint->impulse;

		pSrcPoint++;

		pDstPointA++;
		pDstPointB++;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 通知
	////////////////////////////////////////////////////////////////////////////////////////////////////

	/*
		オブジェクトA
	*/

	pObjWrapA->pInternalObject->NotifyContact( pObjWrapA->pObject, pObjWrapB->pObject, pointCount, pointsA );

	/*
		オブジェクトB
	*/

	pObjWrapB->pInternalObject->NotifyContact( pObjWrapB->pObject, pObjWrapA->pObject, pointCount, pointsB );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// DynamicsWorld::InternalTestCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

DynamicsWorld::InternalTestCallback::InternalTestCallback( Mix::Scene::IDynamicsWorld::TestCallback* pCallback )
{
	MIX_ASSERT( pCallback != NULL );
	m_pCallback = pCallback;
}

DynamicsWorld::InternalTestCallback::~InternalTestCallback( void )
{
}

Boolean DynamicsWorld::InternalTestCallback::OnHit( Mix::Dynamics::IObject* pObject )
{
	MIX_ASSERT( pObject->GetUserPtr() != NULL );

	Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER* pObjWrap = static_cast<Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER*>( pObject->GetUserPtr() );

	return m_pCallback->OnHit( pObjWrap->pObject );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// DynamicsWorld
////////////////////////////////////////////////////////////////////////////////////////////////////

DynamicsWorld* DynamicsWorld::CreateInstance( const Mix::Dynamics::WORLD_CONFIG& config )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_SCENE, DynamicsWorld, config );
}

DynamicsWorld::DynamicsWorld( const Mix::Dynamics::WORLD_CONFIG& config ) :
m_Config( config ),
m_pWorld( NULL ),
m_pContactProc( NULL )
{
	m_Config.persistentManifoldPoolSize = max( 4096, m_Config.persistentManifoldPoolSize );
	m_Config.collisionAlgorithmPoolSize = max( 4096, m_Config.collisionAlgorithmPoolSize );
}

DynamicsWorld::~DynamicsWorld( void )
{
	Dispose();
}

Boolean DynamicsWorld::Initialize( const wchar_t* pDebugName )
{
	MIX_ASSERT( pDebugName != NULL );
	MIX_ASSERT( m_pWorld == NULL );

	Mix::Dynamics::IManager* pManager = Mix::Dynamics::GetManagerPtr();

	if( pManager != NULL )
	{
		if( pManager->CreateWorld( m_Config, &m_pWorld, pDebugName ) == False )
		{
			return False;
		}

		m_pContactProc = DynamicsWorld::ContactProcessor::CreateInstance();
		if( m_pContactProc == NULL )
		{
			return False;
		}
	}
	else
	{
		m_pWorld = NULL;
		m_pContactProc = NULL;
	}

	RendererObject::SetRendering( True );

	return True;
}

void DynamicsWorld::Dispose( void )
{
	MIX_RELEASE( m_pContactProc );
	MIX_RELEASE( m_pWorld );

	RendererObject::SetRendering( False );
}

Mix::Dynamics::IObjectDragger* DynamicsWorld::CreateObjectDragger( const wchar_t* pDebugName )
{
	Mix::Dynamics::IManager* pManager = Mix::Dynamics::GetManagerPtr();
	if( pManager == NULL )
	{
		return NULL;
	}

	Mix::Dynamics::IObjectDragger* pObjectDragger = NULL;

	MIX_ASSERT( m_pWorld != NULL );

	if( pManager->CreateObjectDragger( m_pWorld, &pObjectDragger, MIX_SAFE_NAME( pDebugName ) ) != False )
	{
		pObjectDragger->SetCatchFilter( Mix::Dynamics::OF_DEFAULT );
	}

	return pObjectDragger;
}

Mix::Dynamics::IWorld* DynamicsWorld::GetInternalWorldPtr( void ) const
{
	return m_pWorld;
}

Mix::Dynamics::IObjectListener* DynamicsWorld::GetInternalObjectListenerPtr( void ) const
{
	return m_pContactProc;
}

const Mix::Vector3& DynamicsWorld::GetGravity( void ) const
{
	if( m_pWorld == NULL )
	{
		return Mix::Vector3::Zero();
	}

	return m_pWorld->GetGravity();
}

void DynamicsWorld::SetGravity( const Mix::Vector3& gravity )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->SetGravity( gravity );
	}
}

UInt32 DynamicsWorld::Activate( void )
{
	if( m_pWorld == NULL )
	{
		return 0;
	}

	return m_pWorld->Activate();
}

UInt32 DynamicsWorld::Deactivate( Boolean bForce )
{
	if( m_pWorld == NULL )
	{
		return 0;
	}

	return m_pWorld->Deactivate( bForce );
}

Boolean DynamicsWorld::TestRay(	UInt16 filterMask,
								const Mix::Vector3& fromWorldPos,
								const Mix::Vector3& toWorldPos,
								Mix::Scene::IDynamicsWorld::TestCallback* pCallback,
								Mix::Scene::IDynamicsWorld::TEST_RESULT& result )
{
	if( m_pWorld == NULL )
	{
		return False;
	}

	Mix::Dynamics::IWorld::TEST_RESULT internalResult;

	if( m_pWorld->TestRay(	fromWorldPos,
							toWorldPos,
							0xFFFF,
							filterMask,
							( pCallback != NULL )? &DynamicsWorld::InternalTestCallback( pCallback ) : NULL,
							internalResult ) == True )
	{
		Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER* pObjWrap = static_cast<Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER*>( result.pObject->GetUserPtr() );
		Mix::Dynamics::IObject* pInternalObject = pObjWrap->pInternalObject->GetInternalObjectPtr();

		result.pObject = pObjWrap->pObject;

		if( pInternalObject->GetType() == Mix::Dynamics::IObject::STATIC_MESH )
		{
			Mix::Dynamics::IStaticMesh* pInternalStaticMesh = static_cast<Mix::Dynamics::IStaticMesh*>( pInternalObject );
			result.material = pInternalStaticMesh->GetMaterial( internalResult.partIndex, internalResult.polygonIndex );
		}
		else
		{
			result.material = pInternalObject->GetMaterial();
		}

		result.worldPos = internalResult.worldPos;
		result.worldNormal= internalResult.worldNormal;
	}
	else
	{
		return False;
	}

	return True;
}

Boolean DynamicsWorld::TestSweep(	UInt16 filterMask,
									Mix::Dynamics::IShape* pShape,
									const Mix::Vector3& fromWorldPos,
									const Mix::Vector3& toWorldPos,
									Mix::Scene::IDynamicsWorld::TestCallback* pCallback,
									Mix::Scene::IDynamicsWorld::TEST_RESULT& result )
{
	if( m_pWorld == NULL )
	{
		return False;
	}

	Mix::Dynamics::IWorld::TEST_RESULT internalResult;

	if( m_pWorld->TestSweep(	pShape,
								fromWorldPos,
								toWorldPos,
								0xFFFF,
								filterMask,
								( pCallback != NULL )? &DynamicsWorld::InternalTestCallback( pCallback ) : NULL,
								internalResult ) == True )
	{
		Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER* pObjWrap = static_cast<Mix::Scene::Common::DynamicsObject::OBJECT_WRAPPER*>( result.pObject->GetUserPtr() );
		Mix::Dynamics::IObject* pInternalObject = pObjWrap->pInternalObject->GetInternalObjectPtr();

		result.pObject = pObjWrap->pObject;

		if( pInternalObject->GetType() == Mix::Dynamics::IObject::STATIC_MESH )
		{
			Mix::Dynamics::IStaticMesh* pInternalStaticMesh = static_cast<Mix::Dynamics::IStaticMesh*>( pInternalObject );
			result.material = pInternalStaticMesh->GetMaterial( internalResult.partIndex, internalResult.polygonIndex );
		}
		else
		{
			result.material = pInternalObject->GetMaterial();
		}

		result.worldPos = internalResult.worldPos;
		result.worldNormal= internalResult.worldNormal;
	}
	else
	{
		return False;
	}

	return True;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Mix::Scene::IRendererObject
////////////////////////////////////////////////////////////////////////////////////////////////////

Mix::Scene::IRendererObject::TYPE DynamicsWorld::GetType( void ) const
{
	return Mix::Scene::IRendererObject::DYNAMICS_WORLD;
}

Boolean DynamicsWorld::IsRendering( void ) const
{
	return RendererObject::IsRendering();
}

}}}
