#include "Mix/Private/Dynamics/HingeJoint.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* HingeJoint::FAILED_CREATE = L"ヒンジジョイントの作成に失敗";
const Float32 HingeJoint::DEBUG_DRAW_STEP = 10.0f * SIMD_RADS_PER_DEG;

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

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

HingeJoint::HingeJoint(	Mix::Dynamics::IRigidBody* pRigidBodyA,
									const Mix::Vector3& pivotA,
									const Mix::Vector3& axis ) :
m_pObject( NULL ),
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( NULL ),
m_PivotA( pivotA ),
m_PivotB( pivotA ),
m_Axis( axis ),
m_LowerLimit( -MIX_QUARTER_PI ),
m_UpperLimit( +MIX_QUARTER_PI ),
m_ParamFlags( 0 )
{
	MIX_ADD_REF( m_pRigidBodyA );
}

HingeJoint::HingeJoint(	Mix::Dynamics::IRigidBody* pRigidBodyA,
									Mix::Dynamics::IRigidBody* pRigidBodyB,
									const Mix::Vector3& pivotA,
									const Mix::Vector3& pivotB,
									const Mix::Vector3& axis ) :
m_pObject( NULL ),
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( pRigidBodyB ),
m_PivotA( pivotA ),
m_PivotB( pivotB ),
m_Axis( axis ),
m_LowerLimit( -MIX_QUARTER_PI ),
m_UpperLimit( +MIX_QUARTER_PI ),
m_ParamFlags( 0 )
{
	MIX_ADD_REF( m_pRigidBodyA );
	MIX_ADD_REF( m_pRigidBodyB );
}

HingeJoint::~HingeJoint( void )
{
	MIX_LIB_DELETE( m_pObject );
	MIX_RELEASE( m_pRigidBodyA );
	MIX_RELEASE( m_pRigidBodyB );
}

Boolean HingeJoint::Initialize( const wchar_t* pDebugName )
{
	btTransform frameA;
	btTransform frameB;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// フレームの設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	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();

		Bullet_ComputeFrames(	rbA,
								rbB,
								btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ),
								btVector3( m_PivotB.x, m_PivotB.y, m_PivotB.z ),
								btVector3( m_Axis.x, m_Axis.y, m_Axis.z ),
								frameA,
								frameB );

		m_pObject = MIX_LIB_NEW btHingeConstraint( *rbA, *rbB, frameA, frameB );
	}
	else
	{
		btRigidBody* rbA = static_cast<Mix::Dynamics::RigidBody*>( m_pRigidBodyA )->Bullet_GetRigidBodyPtr();

		Bullet_ComputeFrames(	rbA,
								btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ),
								btVector3( m_Axis.x, m_Axis.y, m_Axis.z ),
								frameA,
								frameB );

		m_pObject = MIX_LIB_NEW btHingeConstraint( *rbA, btTypedConstraint::getFixedBody(), frameA, frameB );
	}

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

	m_pObject->buildJacobian();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// パラメータの初期化
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& pivotA = frameA.getOrigin();
	const btVector3& pivotB = frameB.getOrigin();

	m_PivotA.x = pivotA.x();
	m_PivotA.y = pivotA.y();
	m_PivotA.z = pivotA.z();

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

	m_pObject->setLimit( m_LowerLimit, m_UpperLimit );

	m_pObject->enableMotor( false );
	m_pObject->setMotorTarget( 0.0f, 0.0f );
	m_pObject->setMotorTarget( btQuaternion( 0.0f, 0.0f, 0.0f, 1.0f ), 0.0f );
	m_pObject->setMaxMotorImpulse( 0.0f );

	return True;
}

void HingeJoint::UpdatePivots( void )
{
	btTransform frameA = m_pObject->getAFrame();
	btTransform frameB = m_pObject->getBFrame();

	frameA.getOrigin().setValue( m_PivotA.x, m_PivotA.y, m_PivotA.z );
	frameB.getOrigin().setValue( m_PivotB.x, m_PivotB.y, m_PivotB.z );

	m_pObject->setFrames( frameA, frameB );
}

void HingeJoint::UpdateFrames( void )
{
	btTransform frameA;
	btTransform frameB;

	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();

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

		Bullet_ComputeFrames(	rbA,
								btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ),
								btVector3( m_Axis.x, m_Axis.y, m_Axis.z ),
								frameA,
								frameB );
	}

	m_pObject->setFrames( frameA, frameB );
}

void HingeJoint::Bullet_ComputeFrames(	const btRigidBody* rbA,
										const btVector3& pivotA,
										const btVector3& axis,
										btTransform& frameA,
										btTransform& frameB )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Aフレーム( リジッドボディＡの空間 )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& rbAxisA0 = axis;
	btVector3 rbAxisA1;
	btVector3 rbAxisA2;

	btPlaneSpace1( rbAxisA0, rbAxisA1, rbAxisA2 );

	frameA.getOrigin() = pivotA;
	frameA.getBasis().setValue(	rbAxisA1.getX(), rbAxisA2.getX(), rbAxisA0.getX(),
									rbAxisA1.getY(), rbAxisA2.getY(), rbAxisA0.getY(),
									rbAxisA1.getZ(), rbAxisA2.getZ(), rbAxisA0.getZ() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Bフレーム( ワールド空間 )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( rbA->isKinematicObject() == true )
	{
		btTransform tr;

		rbA->getMotionState()->getWorldTransform( tr );

		frameB = tr * frameA;
	}
	else
	{
		frameB = rbA->getCenterOfMassTransform() * frameA;
	}
}

void HingeJoint::Bullet_ComputeFrames(	const btRigidBody* rbA,
										const btRigidBody* rbB,
										const btVector3& pivotA,
										const btVector3& pivotB,
										const btVector3& axis,
										btTransform& frameA,
										btTransform& frameB )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Aフレーム
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& rbAxisA0 = axis;
	btVector3 rbAxisA1;
	btVector3 rbAxisA2;

	btPlaneSpace1( rbAxisA0, rbAxisA1, rbAxisA2 );

	frameA.getOrigin() = pivotA;
	frameA.getBasis().setValue(	rbAxisA1.getX(), rbAxisA2.getX(), rbAxisA0.getX(),
									rbAxisA1.getY(), rbAxisA2.getY(), rbAxisA0.getY(),
									rbAxisA1.getZ(), rbAxisA2.getZ(), rbAxisA0.getZ() );

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

	btMatrix3x3 basisA;
	btMatrix3x3 basisB;

	if( rbA->isKinematicObject() == true )
	{
		btTransform tr;

		rbA->getMotionState()->getWorldTransform( tr );
		basisA = tr.getBasis();
	}
	else
	{
		basisA = rbA->getCenterOfMassTransform().getBasis();
	}

	if( rbB->isKinematicObject() == true )
	{
		btTransform tr;

		rbB->getMotionState()->getWorldTransform( tr );
		basisB = tr.getBasis();
	}
	else
	{
		basisB = rbB->getCenterOfMassTransform().getBasis();
	}

	frameB.getOrigin() = pivotB;
	frameB.getBasis() = basisA * frameA.getBasis();
	frameB.getBasis() = basisB.inverse() * frameB.getBasis();
}

Mix::Dynamics::IJoint::TYPE HingeJoint::GetType( void ) const
{
	return Mix::Dynamics::IJoint::HINGE;
}

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

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

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

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

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

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

void HingeJoint::GetRigidBodyA( Mix::Dynamics::IRigidBody** ppRigidBody )
{
	MIX_ADD_REF( m_pRigidBodyA );
	( *ppRigidBody ) = m_pRigidBodyA;
}

void HingeJoint::GetRigidBodyB( Mix::Dynamics::IRigidBody** ppRigidBody )
{
	MIX_ADD_REF( m_pRigidBodyB );
	( *ppRigidBody ) = m_pRigidBodyB;
}

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

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

	UpdatePivots();
}

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

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

	UpdatePivots();
}

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

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

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

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

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

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

void HingeJoint::Debug_Draw( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer, Float32 opacity )
{
	Mix::Matrix4x4 oldMat = pPerspectiveRenderer->GetMatrix();
	Mix::Vector4 oldColor = pPerspectiveRenderer->GetColor();

	const btTransform& frameA = m_pObject->getAFrame();
	const btTransform& frameB = m_pObject->getBFrame();
	const btVector3& pivotA = frameA.getOrigin();
	const btVector3& pivotB = frameB.getOrigin();
	const btRigidBody* pRigidBodyA = &( m_pObject->getRigidBodyA() );
	const btRigidBody* pRigidBodyB = &( m_pObject->getRigidBodyB() );

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

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

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

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

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

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リミット
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( MIX_TESTBIT( m_DebugDrawFlags, Mix::Dynamics::DD_JOINT_LIMIT ) == Mix::Dynamics::DD_JOINT_LIMIT )
	{
		Float32 minAngle = m_pObject->getLowerLimit();
		Float32 maxAngle = m_pObject->getUpperLimit();

		btTransform tr;

		if( pRigidBodyB->isKinematicObject() == true )
		{
			btTransform mstr;
			pRigidBodyB->getMotionState()->getWorldTransform( mstr );
			tr = mstr * m_pObject->getBFrame();
		}
		else
		{
			tr = pRigidBodyB->getCenterOfMassTransform() * m_pObject->getBFrame();
		}

		pPerspectiveRenderer->SetColor( Mix::Dynamics::Debug::GetColor( Mix::Dynamics::DDC_JOINT_LIMIT, opacity ) );

		Debug::DrawArc( pPerspectiveRenderer,
						tr.getOrigin(),
						tr.getBasis().getColumn( 2 ),
						tr.getBasis().getColumn( 0 ),
						m_DebugDrawLimitScaling,
						m_DebugDrawLimitScaling,
						minAngle,
						maxAngle );
	}

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

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

const Mix::Vector3& HingeJoint::GetAxis( void ) const
{
	return m_Axis;
}

void HingeJoint::SetAxis( const Mix::Vector3& axis )
{
	m_Axis = axis;

	UpdateFrames();
}

Float32 HingeJoint::GetLowerLimit( void ) const
{
	return m_LowerLimit;
}

void HingeJoint::SetLowerLimit( Float32 limit )
{
	m_LowerLimit = limit;
	m_pObject->setLimit( m_LowerLimit, m_UpperLimit );
}

Float32 HingeJoint::GetUpperLimit( void ) const
{
	return m_UpperLimit;
}

void HingeJoint::SetUpperLimit( Float32 limit )
{
	m_UpperLimit = limit;
	m_pObject->setLimit( m_LowerLimit, m_UpperLimit );
}

Boolean HingeJoint::GetMotorEnabled( void ) const
{
	return ( m_pObject->getEnableAngularMotor() == true )? True : False;
}

void HingeJoint::SetMotorEnabled( Boolean state )
{
	m_pObject->enableMotor( ( state == True ) );
}

void HingeJoint::SetMotorParams( Float32 velocity, Float32 maxImpulse )
{
	m_pObject->enableAngularMotor( m_pObject->getEnableAngularMotor(), velocity, maxImpulse );
}

void HingeJoint::SetMotorParams( Float32 angle, Float32 dt, Float32 maxImpulse )
{
	m_pObject->setMotorTarget( angle, dt );
	m_pObject->setMaxMotorImpulse( maxImpulse );
}

void HingeJoint::SetMotorParams( const Mix::Quaternion& ab, Float32 dt, Float32 maxImpulse )
{
	m_pObject->setMotorTarget( btQuaternion( ab.x, ab.y, ab.z, ab.w ), dt );
	m_pObject->setMaxMotorImpulse( maxImpulse );
}

Float32 HingeJoint::GetLimitSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, HingeJoint::PF_LIMIT_SPRING ) != HingeJoint::PF_LIMIT_SPRING )
	{
		return 0.0f;
	}

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

void HingeJoint::SetLimitSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, spring, -1 );
}

Float32 HingeJoint::GetLimitDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, HingeJoint::PF_LIMIT_DAMPER ) != HingeJoint::PF_LIMIT_DAMPER )
	{
		return 0.0f;
	}

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

void HingeJoint::SetLimitDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, damper, -1 );
}

Float32 HingeJoint::GetNormalSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, HingeJoint::PF_NORMAL_SPRING ) != HingeJoint::PF_NORMAL_SPRING )
	{
		return 0.0f;
	}

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

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

Float32 HingeJoint::GetAngle( void ) const
{
	return m_pObject->getHingeAngle();
}

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

}}
