#include "Mix/Private/Dynamics/DofJoint.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* DofJoint::FAILED_CREATE = L"DOFジョイントの作成に失敗";

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

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

DofJoint::DofJoint(	Mix::Dynamics::IRigidBody* pRigidBodyA,
					const Mix::Quaternion& rotA,
					const Mix::Vector3& pivotA ) :
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( NULL ),
m_RotA( rotA ),
m_RotB( rotA ),
m_PivotA( pivotA ),
m_PivotB( pivotA ),
m_pObject( NULL )
{
	MIX_ADD_REF( m_pRigidBodyA );
}

DofJoint::DofJoint(	Mix::Dynamics::IRigidBody* pRigidBodyA,
					Mix::Dynamics::IRigidBody* pRigidBodyB,
					const Mix::Quaternion& rotA,
					const Mix::Quaternion& rotB,
					const Mix::Vector3& pivotA,
					const Mix::Vector3& pivotB ) :
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( pRigidBodyB ),
m_RotA( rotA ),
m_RotB( rotB ),
m_PivotA( pivotA ),
m_PivotB( pivotB ),
m_pObject( NULL )
{
	MIX_ADD_REF( m_pRigidBodyA );
	MIX_ADD_REF( m_pRigidBodyB );
}

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

Boolean DofJoint::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 btGeneric6DofSpringConstraint(	*rbA,
																*rbB,
																btTransform( btQuaternion( m_RotA.x, m_RotA.y, m_RotA.z, m_RotA.w ), btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ) ),
																btTransform( btQuaternion( m_RotB.x, m_RotB.y, m_RotB.z, m_RotB.w ), btVector3( m_PivotB.x, m_PivotB.y, m_PivotB.z ) ),
																false );
	}
	else
	{
		btRigidBody* rbA = static_cast<Mix::Dynamics::RigidBody*>( m_pRigidBodyA )->Bullet_GetRigidBodyPtr();

		m_pObject = MIX_LIB_NEW btGeneric6DofSpringConstraint(	*rbA,
																btTransform( btQuaternion( m_RotA.x, m_RotA.y, m_RotA.z, m_RotA.w ), btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ) ),
																false );
	}

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

	m_pObject->buildJacobian();

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 初期値の設定
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	m_LowerLinearLimit.Set( -1.0f, -1.0f, -1.0f );
	m_UpperLinearLimit.Set( +1.0f, +1.0f, +1.0f );
	m_LinearStiffness.Set( 1.0f, 1.0f, 1.0f );
	m_LinearDamping.Set( 1.0f, 1.0f, 1.0f );
	m_LinearLimitSpring.Set( 0.0f, 0.0f, 0.0f );
	m_LinearLimitDamper.Set( 0.0f, 0.0f, 0.0f );

	m_LowerAngularLimit.Set( MIX_TO_RAD( -45.0f ), MIX_TO_RAD( -45.0f ), MIX_TO_RAD( -45.0f ) );
	m_UpperAngularLimit.Set( MIX_TO_RAD( +45.0f ), MIX_TO_RAD( +45.0f ), MIX_TO_RAD( +45.0f ) );
	m_AngularStiffness.Set( 1.0f, 1.0f, 1.0f );
	m_AngularDamping.Set( 1.0f, 1.0f, 1.0f );
	m_AngularLimitSpring.Set( 0.0f, 0.0f, 0.0f );
	m_AngularLimitDamper.Set( 0.0f, 0.0f, 0.0f );

	m_pObject->setLinearLowerLimit( btVector3( m_LowerLinearLimit.x, m_LowerLinearLimit.y, m_LowerLinearLimit.z ) );
	m_pObject->setLinearUpperLimit( btVector3( m_UpperLinearLimit.x, m_UpperLinearLimit.y, m_UpperLinearLimit.z ) );
	m_pObject->setAngularLowerLimit( btVector3( m_LowerAngularLimit.x, m_LowerAngularLimit.y, m_LowerAngularLimit.z ) );
	m_pObject->setAngularUpperLimit( btVector3( m_UpperAngularLimit.x, m_UpperAngularLimit.y, m_UpperAngularLimit.z ) );
	m_pObject->enableSpring( 0, false );
	m_pObject->enableSpring( 1, false );
	m_pObject->enableSpring( 2, false );
	m_pObject->enableSpring( 3, false );
	m_pObject->enableSpring( 4, false );
	m_pObject->enableSpring( 5, false );
	m_pObject->setStiffness( 0, m_LinearStiffness.x );
	m_pObject->setStiffness( 1, m_LinearStiffness.y );
	m_pObject->setStiffness( 2, m_LinearStiffness.z );
	m_pObject->setStiffness( 3, m_AngularStiffness.x );
	m_pObject->setStiffness( 4, m_AngularStiffness.y );
	m_pObject->setStiffness( 5, m_AngularStiffness.z );
	m_pObject->setDamping( 0, m_LinearDamping.x );
	m_pObject->setDamping( 1, m_LinearDamping.y );
	m_pObject->setDamping( 2, m_LinearDamping.z );
	m_pObject->setDamping( 3, m_AngularDamping.x );
	m_pObject->setDamping( 4, m_AngularDamping.y );
	m_pObject->setDamping( 5, m_AngularDamping.z );
/*
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 0 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 1 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 2 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 0 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 1 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 2 );

	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 3 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 4 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, 0.9f, 5 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 3 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 4 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, 0.1f, 5 );
*/

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 釣り合いポイントの更新
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pObject->setEquilibriumPoint();

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	return True;
}

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

	frameA.setRotation( btQuaternion( m_RotA.x, m_RotA.y, m_RotA.z, m_RotA.w ) );
	frameA.setOrigin( btVector3( m_PivotA.x, m_PivotA.y, m_PivotA.z ) );

	frameB.setRotation( btQuaternion( m_RotB.x, m_RotB.y, m_RotB.z, m_RotB.w ) );
	frameB.setOrigin( btVector3( m_PivotB.x, m_PivotB.y, m_PivotB.z ) );

	m_pObject->setFrames( frameA, frameB );
}

Mix::Dynamics::IJoint::TYPE DofJoint::GetType( void ) const
{
	return Mix::Dynamics::IJoint::DOF;
}

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

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

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

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

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

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

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

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

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

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

	UpdateFrames();
}

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

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

	UpdateFrames();
}

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

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

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

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

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

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

void DofJoint::Debug_Draw( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer, Float32 opacity )
{
	const btVector3& pivotA = m_pObject->getFrameOffsetA().getOrigin();
	const btVector3& pivotB = m_pObject->getFrameOffsetB().getOrigin();

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

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

	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 )
	{
		pPerspectiveRenderer->SetColor( Mix::Dynamics::Debug::GetColor( Mix::Dynamics::DDC_JOINT_LIMIT, opacity ) );

		btTransform tr = m_pObject->getCalculatedTransformA();

		const btVector3& center = m_pObject->getCalculatedTransformB().getOrigin();
		btVector3 up = tr.getBasis().getColumn( 2 );
		btVector3 axis = tr.getBasis().getColumn( 0 );
		btScalar minTh = m_pObject->getRotationalLimitMotor( 1 )->m_loLimit;
		btScalar maxTh = m_pObject->getRotationalLimitMotor( 1 )->m_hiLimit;
		btScalar minPs = m_pObject->getRotationalLimitMotor( 2 )->m_loLimit;
		btScalar maxPs = m_pObject->getRotationalLimitMotor( 2 )->m_hiLimit;

		Debug::DrawSpherePatch(	pPerspectiveRenderer,
								center,
								up,
								axis,
								m_DebugDrawLimitScaling * 0.9f,
								minTh,
								maxTh,
								minPs,
								maxPs );

		axis = tr.getBasis().getColumn( 1 );
		btScalar ay = m_pObject->getAngle( 1 );
		btScalar az = m_pObject->getAngle( 2 );
		btScalar cy = btCos( ay );
		btScalar sy = btSin( ay );
		btScalar cz = btCos( az );
		btScalar sz = btSin( az );
		btVector3 ref;

		ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2];
		ref[1] = -sz * axis[0] + cz * axis[1];
		ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2];
		tr = m_pObject->getCalculatedTransformB();

		btVector3 normal = -tr.getBasis().getColumn( 0 );
		btScalar minFi = m_pObject->getRotationalLimitMotor( 0 )->m_loLimit;
		btScalar maxFi = m_pObject->getRotationalLimitMotor( 0 )->m_hiLimit;

		if( minFi > maxFi )
		{
			Debug::DrawArc( pPerspectiveRenderer,
							center,
							normal,
							ref,
							m_DebugDrawLimitScaling,
							m_DebugDrawLimitScaling,
							-SIMD_PI,
							SIMD_PI,
							False );
		}
		else if( minFi < maxFi )
		{
			Debug::DrawArc( pPerspectiveRenderer,
							center,
							normal,
							ref,
							m_DebugDrawLimitScaling,
							m_DebugDrawLimitScaling,
							minFi,
							maxFi,
							True );
		}

		pPerspectiveRenderer->SetMatrix( ToMixMatrix4x4( m_pObject->getCalculatedTransformA() ) );
		Debug::DrawBox( pPerspectiveRenderer,
						m_pObject->getTranslationalLimitMotor()->m_lowerLimit,
						m_pObject->getTranslationalLimitMotor()->m_upperLimit );
	}

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

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

const Mix::Quaternion& DofJoint::GetRotationA( void ) const
{
	return m_RotA;
}

void DofJoint::SetRotationA( const Mix::Quaternion& rot )
{
	m_RotA = rot;

	UpdateFrames();
}

const Mix::Quaternion& DofJoint::GetRotationB( void ) const
{
	return m_RotB;
}

void DofJoint::SetRotationB( const Mix::Quaternion& rot )
{
	m_RotB = rot;

	UpdateFrames();
}

void DofJoint::SetLinearLimit( const Mix::Vector3& lower, const Mix::Vector3& upper )
{
	m_LowerLinearLimit = lower;
	m_UpperLinearLimit = upper;

	m_pObject->setLinearLowerLimit( btVector3( lower.x, lower.y, lower.z ) );
	m_pObject->setLinearUpperLimit( btVector3( upper.x, upper.y, upper.z ) );
}

const Mix::Vector3& DofJoint::GetLinearLowerLimit( void ) const
{
	return m_LowerLinearLimit;
}

const Mix::Vector3& DofJoint::GetLinearUpperLimit( void ) const
{
	return m_UpperLinearLimit;
}

void DofJoint::SetLinearStiffness( const Mix::Vector3& stiffness )
{
	m_LinearStiffness = stiffness;

	m_pObject->enableSpring( 0, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );
	m_pObject->enableSpring( 1, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );
	m_pObject->enableSpring( 2, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );

	m_pObject->setStiffness( 0, stiffness.x );
	m_pObject->setStiffness( 1, stiffness.y );
	m_pObject->setStiffness( 2, stiffness.z );
}

const Mix::Vector3& DofJoint::GetLinearStiffness( void ) const
{
	return m_LinearStiffness;
}

void DofJoint::SetLinearDamping( const Mix::Vector3& damping )
{
	m_LinearDamping = damping;

	m_pObject->setDamping( 0, damping.x );
	m_pObject->setDamping( 1, damping.y );
	m_pObject->setDamping( 2, damping.z );
}

const Mix::Vector3& DofJoint::GetLinearDamping( void ) const
{
	return m_LinearDamping;
}

const Mix::Vector3& DofJoint::GetLinearLimitSpring( void ) const
{
	return m_LinearLimitSpring;
}

void DofJoint::SetLinearLimitSpring( const Mix::Vector3& spring )
{
	m_LinearLimitSpring = spring;

	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_LinearLimitSpring.x, 0 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_LinearLimitSpring.y, 1 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_LinearLimitSpring.z, 2 );
}

const Mix::Vector3& DofJoint::GetLinearLimitDamper( void ) const
{
	return m_LinearLimitDamper;
}

void DofJoint::SetLinearLimitDamper( const Mix::Vector3& damper )
{
	m_LinearLimitDamper = damper;

	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_LinearLimitDamper.x, 0 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_LinearLimitDamper.y, 1 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_LinearLimitDamper.z, 2 );
}

void DofJoint::SetAngularLimit( const Mix::Vector3& lower, const Mix::Vector3& upper )
{
	m_LowerAngularLimit = lower;
	m_UpperAngularLimit = upper;

	m_pObject->setAngularLowerLimit( btVector3( lower.x, lower.y, lower.z ) );
	m_pObject->setAngularUpperLimit( btVector3( upper.x, upper.y, upper.z ) );
}

const Mix::Vector3& DofJoint::GetAngularLowerLimit( void ) const
{
	return m_LowerAngularLimit;
}

const Mix::Vector3& DofJoint::GetAngularUpperLimit( void ) const
{
	return m_UpperAngularLimit;
}

void DofJoint::SetAngularStiffness( const Mix::Vector3& stiffness )
{
	m_AngularStiffness = stiffness;

	m_pObject->enableSpring( 3, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );
	m_pObject->enableSpring( 4, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );
	m_pObject->enableSpring( 5, ( MIX_FLOAT_IS_ZERO( stiffness.x ) == false ) );

	m_pObject->setStiffness( 3, stiffness.x );
	m_pObject->setStiffness( 4, stiffness.y );
	m_pObject->setStiffness( 5, stiffness.z );
}

const Mix::Vector3& DofJoint::GetAngularStiffness( void ) const
{
	return m_AngularStiffness;
}

void DofJoint::SetAngularDamping( const Mix::Vector3& damping )
{
	m_AngularDamping = damping;

	m_pObject->setDamping( 3, damping.x );
	m_pObject->setDamping( 4, damping.y );
	m_pObject->setDamping( 5, damping.z );
}

const Mix::Vector3& DofJoint::GetAngularDamping( void ) const
{
	return m_AngularDamping;
}

const Mix::Vector3& DofJoint::GetAngularLimitSpring( void ) const
{
	return m_AngularLimitSpring;
}

void DofJoint::SetAngularLimitSpring( const Mix::Vector3& spring )
{
	m_AngularLimitSpring = spring;

	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_AngularLimitSpring.x, 3 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_AngularLimitSpring.y, 4 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, m_AngularLimitSpring.z, 5 );
}

const Mix::Vector3& DofJoint::GetAngularLimitDamper( void ) const
{
	return m_AngularLimitSpring;
}

void DofJoint::SetAngularLimitDamper( const Mix::Vector3& damper )
{
	m_AngularLimitDamper = damper;

	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_AngularLimitDamper.x, 3 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_AngularLimitDamper.y, 4 );
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, m_AngularLimitDamper.z, 5 );
}

void DofJoint::UpdateEquilibriumPoint( void )
{
	m_pObject->setEquilibriumPoint();
}

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

}}
