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

#include "Mix/Dynamics/IWorld.h"
#include "Mix/Private/Dynamics/PointJoint.h"
#include "Mix/Private/Dynamics/HingeJoint.h"
#include "Mix/Private/Dynamics/BallJoint.h"
#include "Mix/Private/Scene/Common/ActorRevision.h"
#include "Mix/Private/Scene/Common/ActorNode.h"
#include "Mix/Private/Scene/Common/ActorCollider.h"

namespace Mix{ namespace Scene{ namespace Common{

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

ActorDynamicsPart::ActorDynamicsPart( void ) :
m_bJointCollisionDisabled( False ),
m_pJoint( NULL )
{
}

ActorDynamicsPart::~ActorDynamicsPart( void )
{
	MIX_RELEASE( m_pJoint );
}

void ActorDynamicsPart::SetJoint( Boolean bCollisionDisabled, const Mix::Matrix4x4& localPivotB, Mix::Dynamics::IJoint* pJoint )
{
	MIX_ASSERT( pJoint != NULL );
	MIX_ASSERT( m_pJoint == NULL );

	m_bJointCollisionDisabled = bCollisionDisabled;
	m_JointLocalPivotB = localPivotB;

	MIX_ADD_REF( pJoint );
	m_pJoint = pJoint;
}

Mix::Dynamics::IJoint* ActorDynamicsPart::GetJointPtr( void ) const
{
	return m_pJoint;
}

ActorDynamicsPart* ActorDynamicsPart::Clone( void )
{
	ActorDynamicsPart* pDstPart = ActorDynamicsPart::CreateInstance();

	if( pDstPart != NULL )
	{
		if( ActorDynamicsCluster::Clone( pDstPart ) == False )
		{
			MIX_RELEASE( pDstPart );
		}
	}
	else
	{
		MIX_RELEASE( pDstPart );
	}

	return pDstPart;
}

Boolean ActorDynamicsPart::CloneFinish( std::map<Mix::Dynamics::IRigidBody*, Mix::Dynamics::IRigidBody*>& rbPair, ActorDynamicsPart* pCloneCluster, const wchar_t* pDebugName )
{
	MIX_ASSERT( pCloneCluster != NULL );

	if( m_pJoint == NULL )
	{
		return True;
	}

	MIX_ASSERT( pCloneCluster->GetColliderPtr() != NULL );
	MIX_ASSERT( pCloneCluster->GetColliderPtr()->GetInternalRigidBodyPtr() != NULL );
	MIX_ASSERT( m_pCollider != NULL );
	MIX_ASSERT( m_pCollider->GetInternalRigidBodyPtr() != NULL );
	
	Mix::Dynamics::IJoint::TYPE type = m_pJoint->GetType();
	Boolean bSingle = m_pJoint->IsSingle();

	Mix::Dynamics::IRigidBody* pRigidBodyA = pCloneCluster->GetColliderPtr()->GetInternalRigidBodyPtr();
	Mix::Dynamics::IRigidBody* pRigidBodyB = NULL;

	Mix::Dynamics::IJoint* pCloneJoint = NULL;

	if( bSingle == False )
	{
		Mix::Dynamics::IRigidBody* pSrcRigidBodyB = NULL;
		std::map<Mix::Dynamics::IRigidBody*, Mix::Dynamics::IRigidBody*>::iterator it_rb;

		m_pJoint->GetRigidBodyB( &pSrcRigidBodyB );
		MIX_ASSERT( pSrcRigidBodyB != NULL );

		it_rb = rbPair.find( pSrcRigidBodyB );
		MIX_ASSERT( it_rb != rbPair.end() );

		pRigidBodyB = it_rb->second;

		MIX_RELEASE( pSrcRigidBodyB );
	}

	if( type == Mix::Dynamics::IJoint::POINT )
	{
		Mix::Dynamics::IPointJoint* pSrcJoint = static_cast<Mix::Dynamics::IPointJoint*>( m_pJoint );
		Mix::Dynamics::PointJoint* pDstJoint = NULL;

		if( bSingle == True )
		{
			pDstJoint = Mix::Dynamics::PointJoint::CreateInstance( pRigidBodyA, m_pJoint->GetPivotA() );
		}
		else
		{
			pDstJoint = Mix::Dynamics::PointJoint::CreateInstance( pRigidBodyA, pRigidBodyB, m_pJoint->GetPivotA(), m_pJoint->GetPivotB() );
		}

		if( pDstJoint != NULL )
		{
			if( pDstJoint->Initialize( pDebugName ) == True )
			{
				Float32 pivotSpring = pSrcJoint->GetPivotSpring();
				Float32 pivotDamper = pSrcJoint->GetPivotDamper();

				if( MIX_FLOAT_IS_ZERO( pivotSpring ) == False )
				{
					pDstJoint->SetPivotSpring( pivotSpring );
				}

				if( MIX_FLOAT_IS_ZERO( pivotDamper ) == False )
				{
					pDstJoint->SetPivotDamper( pivotDamper );
				}

				pCloneJoint = pDstJoint;
			}
			else
			{
				MIX_RELEASE( pDstJoint );
				return False;
			}
		}
	}
	else if( type == Mix::Dynamics::IJoint::HINGE )
	{
		Mix::Dynamics::IHingeJoint* pSrcJoint = static_cast<Mix::Dynamics::IHingeJoint*>( m_pJoint );
		Mix::Dynamics::HingeJoint* pDstJoint = NULL;

		if( bSingle == True )
		{
			pDstJoint = Mix::Dynamics::HingeJoint::CreateInstance( pRigidBodyA, pSrcJoint->GetPivotA(), pSrcJoint->GetAxis() );
		}
		else
		{
			pDstJoint = Mix::Dynamics::HingeJoint::CreateInstance( pRigidBodyA, pRigidBodyB, pSrcJoint->GetPivotA(), pSrcJoint->GetPivotB(), pSrcJoint->GetAxis() );
		}

		if( pDstJoint != NULL )
		{
			if( pDstJoint->Initialize( pDebugName ) == True )
			{
				Float32 limitSpring = pSrcJoint->GetLimitSpring();
				Float32 limitDamper = pSrcJoint->GetLimitDamper();

				pDstJoint->SetUpperLimit( pSrcJoint->GetUpperLimit() );
				pDstJoint->SetLowerLimit( pSrcJoint->GetLowerLimit() );

				if( MIX_FLOAT_IS_ZERO( limitSpring ) == False )
				{
					pDstJoint->SetLimitSpring( limitSpring );
				}

				if( MIX_FLOAT_IS_ZERO( limitDamper ) == False )
				{
					pDstJoint->SetLimitDamper( limitDamper );
				}

				pCloneJoint = pDstJoint;
			}
			else
			{
				MIX_RELEASE( pDstJoint );
				return False;
			}
		}
	}
	else if( type == Mix::Dynamics::IJoint::BALL )
	{
		Mix::Dynamics::IBallJoint* pSrcJoint = static_cast<Mix::Dynamics::IBallJoint*>( m_pJoint );
		Mix::Dynamics::BallJoint* pDstJoint = NULL;

		if( bSingle == True )
		{
			pDstJoint = Mix::Dynamics::BallJoint::CreateInstance( pRigidBodyA, pSrcJoint->GetPivotA(), pSrcJoint->GetTwistAxis(), pSrcJoint->GetSwingAxis() );
		}
		else
		{
			pDstJoint = Mix::Dynamics::BallJoint::CreateInstance( pRigidBodyA, pRigidBodyB, pSrcJoint->GetPivotA(), pSrcJoint->GetPivotB(), pSrcJoint->GetTwistAxis(), pSrcJoint->GetSwingAxis() );
		}

		if( pDstJoint != NULL )
		{
			if( pDstJoint->Initialize( pDebugName ) == True )
			{
				Float32 pivotSpring = pSrcJoint->GetPivotSpring();
				Float32 pivotDamper = pSrcJoint->GetPivotDamper();
				Float32 limitSpring = pSrcJoint->GetLimitSpring();
				Float32 limitDamper = pSrcJoint->GetLimitDamper();

				pDstJoint->SetTwistLimit( pSrcJoint->GetTwistLimit() );
				pDstJoint->SetSwingLimit1( pSrcJoint->GetSwingLimit1() );
				pDstJoint->SetSwingLimit2( pSrcJoint->GetSwingLimit2() );

				if( MIX_FLOAT_IS_ZERO( pivotSpring ) == False )
				{
					pDstJoint->SetPivotSpring( pivotSpring );
				}

				if( MIX_FLOAT_IS_ZERO( pivotDamper ) == False )
				{
					pDstJoint->SetPivotDamper( pivotDamper );
				}

				if( MIX_FLOAT_IS_ZERO( limitSpring ) == False )
				{
					pDstJoint->SetLimitSpring( limitSpring );
				}

				if( MIX_FLOAT_IS_ZERO( limitDamper ) == False )
				{
					pDstJoint->SetLimitDamper( limitDamper );
				}

				pCloneJoint = pDstJoint;
			}
			else
			{
				MIX_RELEASE( pDstJoint );
				return False;
			}
		}
	}
	else
	{
		MIX_ERROR( L"アクターダイナミクスクラスターの複製に失敗 : サポートされていないジョイントが定義されています" );
	}

	if( pCloneJoint == NULL )
	{
		return False;
	}

	pCloneCluster->m_pJoint = pCloneJoint;

	return True;
}

void ActorDynamicsPart::OnColliderStateChanged( Boolean state )
{
	if( ( m_pWorld != NULL ) &&( m_pJoint != NULL ) )
	{
		if( state == True )
		{
			MIX_ASSERT( m_pJoint->IsInWorld() == False );
			m_pWorld->AddJoint( m_pJoint, m_bJointCollisionDisabled );
		}
		else
		{
			MIX_ASSERT( m_pJoint->IsInWorld() == True );
			m_pWorld->RemoveJoint( m_pJoint );
		}
	}
}

void ActorDynamicsPart::OnColliderModeChanged( Mix::Scene::DYNAMICS_COLLIDER_MODE mode )
{
	if( m_pJoint != NULL )
	{
		if( ( mode == Mix::Scene::DC_STATIC ) ||
			( mode == Mix::Scene::DC_KINEMATIC ) )
		{
			m_pJoint->SetEnabled( False );
		}
		else
		{
			m_pJoint->SetEnabled( True );
		}
	}
}

void ActorDynamicsPart::SetLink( Mix::Scene::IRendererObject* pOwner, Mix::Matrix4x4* pParentWorldMat, Mix::Matrix4x4* pWorldMat )
{
	MIX_ASSERT( pOwner != NULL );
	MIX_ASSERT( pOwner->GetType() == Mix::Scene::IRendererObject::ACTOR_NODE );

	Mix::Scene::Common::ActorNode* pNode = static_cast<Mix::Scene::Common::ActorNode*>( pOwner );

	ActorDynamicsCluster::SetLink( pOwner, pParentWorldMat, pWorldMat );

	MIX_ASSERT( pNode->GetRevisionPtr() != NULL );
	m_pRevision = pNode->GetRevisionPtr();
}

void ActorDynamicsPart::AttachToWorld( Mix::Dynamics::IWorld* pWorld, Mix::Dynamics::IObjectListener* pObjectListener )
{
	ActorDynamicsCluster::AttachToWorld( pWorld, pObjectListener );

	if( ( IsColliderEnabled() == True ) &&
		( m_pJoint != NULL ) )
	{
		pWorld->AddJoint( m_pJoint, m_bJointCollisionDisabled );
	}
}

void ActorDynamicsPart::DetachFromWorld( Mix::Dynamics::IWorld* pWorld )
{
	ActorDynamicsCluster::DetachFromWorld( pWorld );

	if( ( IsColliderEnabled() == True ) &&
		( m_pJoint != NULL ) )
	{
		pWorld->RemoveJoint( m_pJoint );
	}
}

void ActorDynamicsPart::ResetChild( const Mix::Matrix4x4& baseMat )
{
	ActorDynamicsCluster::ResetChild( baseMat );
	ActorDynamicsPart::UpdateChild( baseMat );
}

Boolean ActorDynamicsPart::NeedsUpdateChild( void ) const
{
	if( ( m_pJoint != NULL ) &&
		( m_pJoint->IsSingle() == True ) )
	{
		return True;
	}

	return ActorDynamicsCluster::NeedsUpdateChild();
}

void ActorDynamicsPart::UpdateChild( const Mix::Matrix4x4& baseMat )
{
	( void )baseMat;

	ActorDynamicsCluster::UpdateChild( baseMat );

	if( ( m_pJoint != NULL ) &&
		( m_pJoint->IsSingle() == True ) )
	{
		//シングルの場合は、以降に行われるワールドシミュレーションのためにピボットＢを設定しておく

		MIX_ASSERT( m_pParentWorldMat != NULL );

		Mix::Matrix4x4 pivotB = m_JointLocalPivotB * *m_pParentWorldMat;

		m_pJoint->SetPivotB( pivotB.GetTranslation() );
	}
}

#ifdef _DEBUG

void ActorDynamicsPart::Debug_Draw(	UInt32 flags,
									Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
									Float32 axisScaling,
									Float32 jointFrameMinSize,
									Float32 jointLimitScaling )
{
	ActorDynamicsCluster::Debug_Draw( flags, pPerspectiveRenderer, axisScaling, jointFrameMinSize, jointLimitScaling );

	if( ( m_pJoint != NULL ) &&
		( MIX_TESTBIT( flags, Mix::Scene::DDF_ACTORMODEL_JOINT ) == Mix::Scene::DDF_ACTORMODEL_JOINT ) )
	{
		Float32 opacity = ( m_pJoint->IsInWorld() == True )? 1.0f : 0.25f;

		m_pJoint->Debug_SetDrawFrameMinSize( jointFrameMinSize );
		m_pJoint->Debug_SetDrawLimitScaling( jointLimitScaling );
		m_pJoint->Debug_Draw( pPerspectiveRenderer, opacity );
	}
}

#endif //_DEBUG

}}}
