#include "Mix/Private/Dynamics/PointJoint.h"

#include "Mix/Private/Dynamics/Utility.h"
#include "Mix/Private/Dynamics/RigidBody.h"

#include "Mix/Graphics/Utility/IPerspectiveRenderer.h"

namespace Mix{ namespace Dynamics{

const wchar_t* PointJoint::FAILED_CREATE = L"ポイントジョイントの作成に失敗";

PointJoint* PointJoint::CreateInstance( Mix::Dynamics::IRigidBody* pRigidBodyA,
										const Mix::Vector3& posA )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, PointJoint, pRigidBodyA, posA );
}

PointJoint* PointJoint::CreateInstance( Mix::Dynamics::IRigidBody* pRigidBodyA,
										Mix::Dynamics::IRigidBody* pRigidBodyB,
										const Mix::Vector3& pivotA,
										const Mix::Vector3& pivotB )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, PointJoint, pRigidBodyA, pRigidBodyB, pivotA, pivotB );
}

PointJoint::PointJoint( Mix::Dynamics::IRigidBody* pRigidBodyA,
						const Mix::Vector3& pivotA ) :
m_pObject( NULL ),
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( NULL ),
m_PivotA( pivotA ),
m_PivotB( pivotA ),
m_ParamFlags( 0 )
{
	MIX_ADD_REF( m_pRigidBodyA );
}

PointJoint::PointJoint( Mix::Dynamics::IRigidBody* pRigidBodyA,
						Mix::Dynamics::IRigidBody* pRigidBodyB,
						const Mix::Vector3& pivotA,
						const Mix::Vector3& pivotB ) :
m_pObject( NULL ),
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( pRigidBodyB ),
m_PivotA( pivotA ),
m_PivotB( pivotB ),
m_ParamFlags( 0 )
{
	MIX_ADD_REF( m_pRigidBodyA );
	MIX_ADD_REF( m_pRigidBodyB );
}

PointJoint::~PointJoint( void )
{
	MIX_LIB_DELETE( m_pObject );

	MIX_RELEASE( m_pRigidBodyA );
	MIX_RELEASE( m_pRigidBodyB );
}

Boolean PointJoint::Initialize( const wchar_t* pDebugName )
{
	if( m_pRigidBodyB != NULL )
	{
		btRigidBody* rbA = static_cast<Mix::Dynamics::RigidBody*>( m_pRigidBodyA )->Bullet_GetRigidBodyPtr();
		btRigidBody* rbB = static_cast<Mix::Dynamics::RigidBody*>( m_pRigidBodyB )->Bullet_GetRigidBodyPtr();

		m_pObject = MIX_LIB_NEW btPoint2PointConstraint(	*rbA,
													*rbB,
													btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ),
													btVector3( m_PivotB.x, m_PivotB.y, m_PivotB.z ) );
	}
	else
	{
		btRigidBody* rbA = static_cast<Mix::Dynamics::RigidBody*>( m_pRigidBodyA )->Bullet_GetRigidBodyPtr();

		m_pObject = MIX_LIB_NEW btPoint2PointConstraint(	*rbA,
													btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ) );

		const btVector3& pivotB = m_pObject->getPivotInB();

		m_PivotB.x = pivotB.x();
		m_PivotB.y = pivotB.y();
		m_PivotB.z = pivotB.z();
	}

	if( m_pObject == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : DebugName[%s]", PointJoint::FAILED_CREATE, Mix::STR_OUTOFMEMORY, pDebugName );
		return False;
	}

	m_pObject->buildJacobian();

	return True;
}

Mix::Dynamics::IJoint::TYPE PointJoint::GetType( void ) const
{
	return Mix::Dynamics::IJoint::POINT;
}

Boolean PointJoint::IsInWorld( void ) const
{
	return Joint::IsInWorld();
}

Boolean PointJoint::IsEnabled( void ) const
{
	return m_pObject->isEnabled();
}

void PointJoint::SetEnabled( Boolean state )
{
	return m_pObject->setEnabled( ( state == True ) );
}

Float32 PointJoint::GetBreakingImpulseThreshold( void ) const
{
	return m_pObject->getBreakingImpulseThreshold();
}

void PointJoint::SetBreakingImpulseThreshold( Float32 threshold )
{
	m_pObject->setBreakingImpulseThreshold( threshold );
}

Boolean PointJoint::IsSingle( void ) const
{
	return ( m_pRigidBodyB == NULL );
}

void PointJoint::GetRigidBodyA( Mix::Dynamics::IRigidBody** ppRigidBody )
{
	if( ppRigidBody == NULL )
	{
		return;
	}

	MIX_ADD_REF( m_pRigidBodyA );
	( *ppRigidBody ) = m_pRigidBodyA;
}

void PointJoint::GetRigidBodyB( Mix::Dynamics::IRigidBody** ppRigidBody )
{
	if( ppRigidBody == NULL )
	{
		return;
	}

	MIX_ADD_REF( m_pRigidBodyB );
	( *ppRigidBody ) = m_pRigidBodyB;
}

const Mix::Vector3& PointJoint::GetPivotA( void ) const
{
	return m_PivotA;
}

void PointJoint::SetPivotA( const Mix::Vector3& pivot )
{
	m_PivotA = pivot;

	m_pObject->setPivotA( btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ) );
}

const Mix::Vector3& PointJoint::GetPivotB( void ) const
{
	return m_PivotB;
}

void PointJoint::SetPivotB( const Mix::Vector3& pivot )
{
	m_PivotB = pivot;

	m_pObject->setPivotB( btVector3( m_PivotB.x, m_PivotB.y, m_PivotB.z ) );
}

UInt32 PointJoint::Debug_GetDrawFlags( void ) const
{
	return m_DebugDrawFlags;
}

void PointJoint::Debug_SetDrawFlags( UInt32 flags )
{
	m_DebugDrawFlags = flags;
}

Float32 PointJoint::Debug_GetDrawFrameMinSize( void ) const
{
	return m_DebugDrawFrameMinSize;
}

void PointJoint::Debug_SetDrawFrameMinSize( Float32 minSize )
{
	m_DebugDrawFrameMinSize = minSize;
}

Float32 PointJoint::Debug_GetDrawLimitScaling( void ) const
{
	return m_DebugDrawLimitScaling;
}

void PointJoint::Debug_SetDrawLimitScaling( Float32 scaling )
{
	m_DebugDrawLimitScaling = scaling;
}

void PointJoint::Debug_Draw( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer, Float32 opacity )
{
	const btVector3& pivotA = m_pObject->getPivotInA();
	const btVector3& pivotB = m_pObject->getPivotInB();
	const btRigidBody* pRigidBodyA = &( m_pObject->getRigidBodyA() );
	const btRigidBody* pRigidBodyB = &( m_pObject->getRigidBodyB() );

	Mix::Matrix4x4 oldMat = pPerspectiveRenderer->GetMatrix();
	Mix::Vector4 oldColor = pPerspectiveRenderer->GetColor();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 開始
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pPerspectiveRenderer->SetMatrix( Mix::Matrix4x4::Identity() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// フレーム
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( MIX_TESTBIT( m_DebugDrawFlags, Mix::Dynamics::DDC_JOINT_FRAME ) == Mix::Dynamics::DDC_JOINT_FRAME )
	{
		pPerspectiveRenderer->SetColor( Mix::Dynamics::Debug::GetColor( Mix::Dynamics::DDC_JOINT_FRAME, opacity ) );

		Debug::DrawPivot(	pPerspectiveRenderer,
							m_pObject,
							pivotA,
							pivotB,
							m_DebugDrawFrameMinSize );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 終了
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pPerspectiveRenderer->SetColor( oldColor );
	pPerspectiveRenderer->SetMatrix( oldMat );
}

Float32 PointJoint::GetPivotSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, PointJoint::PF_PIVOT_SPRING ) != PointJoint::PF_PIVOT_SPRING )
	{
		return 0.0f;
	}

	return m_pObject->getParam( BT_CONSTRAINT_CFM, -1 );
}

void PointJoint::SetPivotSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_CFM, spring, -1 );
}

Float32 PointJoint::GetPivotDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, PointJoint::PF_PIVOT_DAMPER ) != PointJoint::PF_PIVOT_DAMPER )
	{
		return 0.0f;
	}

	return m_pObject->getParam( BT_CONSTRAINT_ERP, -1 );
}

void PointJoint::SetPivotDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_ERP, damper, -1 );
}

btTypedConstraint* PointJoint::Bullet_GetTypedConstraintPtr( void ) const
{
	return m_pObject;
}

}}
