#include "Mix/Tool/Win32/Core/Dynamics/Impl/PointJoint.h"

#include "Mix/Tool/Win32/Core/Graphics/LineHelper.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/Types.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/World.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/RigidBody.h"

#include "btBulletDynamicsCommon.h"

namespace Mix{ namespace Tool{ namespace Win32{ namespace Dynamics{ namespace Impl{

PointJoint::PointJoint( void ) :
m_pConstraint( NULL ),
m_pWorld( NULL ),
m_pRigidBodyA( NULL ),
m_pRigidBodyB( NULL ),
m_PivotA( 0.0f, 0.0f, 0.0f ),
m_PivotB( 0.0f, 0.0f, 0.0f ),
m_bCollisionDisabled( false ),
m_ParamFlags( 0 )
{
	for( unsigned int i = 0; i < PointJoint::PT_MAX; i++ )
	{
		m_ParamValues[i] = 0.0f;
	}
}

PointJoint::PointJoint( bool collisionDisabled, float spring, float damper ) :
m_pConstraint( NULL ),
m_pWorld( NULL ),
m_pRigidBodyA( NULL ),
m_pRigidBodyB( NULL ),
m_PivotA( 0.0f, 0.0f, 0.0f ),
m_PivotB( 0.0f, 0.0f, 0.0f ),
m_bCollisionDisabled( collisionDisabled ),
m_ParamFlags( 0 )
{
	for( unsigned int i = 0; i < PointJoint::PT_MAX; i++ )
	{
		m_ParamValues[i] = 0.0f;
	}

	SetSpring( spring );
	SetDamper( damper );
}

PointJoint::~PointJoint( void )
{
	MIX_DELETE( m_pConstraint );
}

void PointJoint::SetWorldPtr( Mix::Tool::Win32::Dynamics::Impl::World* pWorld )
{
	m_pWorld = pWorld;
}

float PointJoint::GetSpring( void ) const
{
	return GetParam( PointJoint::PT_CFM, PointJoint::PA_NONE );
}

void PointJoint::SetSpring( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( PointJoint::PT_CFM, PointJoint::PA_NONE, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( PointJoint::PT_CFM, PointJoint::PA_NONE );
	}
}

float PointJoint::GetDamper( void ) const
{
	return GetParam( PointJoint::PT_ERP, PointJoint::PA_NONE );
}

void PointJoint::SetDamper( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( PointJoint::PT_ERP, PointJoint::PA_NONE, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( PointJoint::PT_ERP, PointJoint::PA_NONE );
	}
}

void PointJoint::ResetInitalRigidBodyTransform( void )
{
}

void PointJoint::SetInitalRigidBodyTransform(	const D3DXQUATERNION& rotA,
												const D3DXVECTOR3& posA,
												const D3DXQUATERNION& rotB,
												const D3DXVECTOR3& posB )
{
}

bool PointJoint::SetFrame(	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyA,
								const D3DXVECTOR3& pivotA )
{
	bool ret = false;

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 作成処理
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	BeginRefresh();

	MIX_DELETE( m_pConstraint );
	m_pRigidBodyA = NULL;
	m_pRigidBodyB = NULL;

	if( ( pRigidBodyA != NULL ) &&
		( pRigidBodyA->GetShapeHandleCount() > 0 ) )
	{
		RigidBody::STATUS statusA = pRigidBodyA->GetStatus();

		if( ( statusA == RigidBody::DEFAULT ) ||
			( statusA == RigidBody::KINEMATIC ) )
		{
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyA = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyA );
			btRigidBody* pBulletRigidBodyA = pImplRigidBodyA->Bullet_GetRigidBodyPtr();

			if( pBulletRigidBodyA != NULL )
			{
				m_pConstraint = new btPoint2PointConstraint( *pBulletRigidBodyA, btVector3( pivotA.x, pivotA.y, pivotA.z ) );
				if( m_pConstraint != NULL )
				{
					const btVector3& pivotB = m_pConstraint->getPivotInB();

					m_pConstraint->buildJacobian();

					m_pRigidBodyA = pRigidBodyA;
					m_PivotB = D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() );

					ret = true;
				}
			}
		}
	}

	UpdateParams();

	EndRefresh();

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

	return ret;
}

bool PointJoint::SetFrame(	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyA,
								Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyB,
								const D3DXVECTOR3& pivotA,
								const D3DXVECTOR3& pivotB )
{
	bool ret = false;

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 作成処理
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	BeginRefresh();

	MIX_DELETE( m_pConstraint );
	m_pRigidBodyA = NULL;
	m_pRigidBodyB = NULL;

	if( ( pRigidBodyA != NULL ) &&
		( pRigidBodyB != NULL ) &&
		( pRigidBodyA->GetShapeHandleCount() > 0 ) &&
		( pRigidBodyB->GetShapeHandleCount() > 0 ) )
	{
		Mix::Tool::Win32::Dynamics::RigidBody::STATUS statusA = pRigidBodyA->GetStatus();
		Mix::Tool::Win32::Dynamics::RigidBody::STATUS statusB = pRigidBodyB->GetStatus();

		if( ( statusA == Mix::Tool::Win32::Dynamics::RigidBody::DEFAULT ) ||
			( statusB == Mix::Tool::Win32::Dynamics::RigidBody::DEFAULT ) ||
			( statusA == Mix::Tool::Win32::Dynamics::RigidBody::KINEMATIC ) ||
			( statusB == Mix::Tool::Win32::Dynamics::RigidBody::KINEMATIC ) )
		{
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyA = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyA );
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyB = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyB );
			btRigidBody* pBulletRigidBodyA = pImplRigidBodyA->Bullet_GetRigidBodyPtr();
			btRigidBody* pBulletRigidBodyB = pImplRigidBodyB->Bullet_GetRigidBodyPtr();

			if( ( pBulletRigidBodyA != NULL ) &&
				( pBulletRigidBodyB != NULL ) )
			{
				m_pConstraint = new btPoint2PointConstraint(	*pBulletRigidBodyA,
																*pBulletRigidBodyB,
																btVector3( pivotA.x, pivotA.y, pivotA.z ),
																btVector3( pivotB.x, pivotB.y, pivotB.z ) );
				if( m_pConstraint != NULL )
				{
					m_pConstraint->buildJacobian();

					m_pRigidBodyA = pRigidBodyA;
					m_pRigidBodyB = pRigidBodyB;
					m_PivotA = pivotA;
					m_PivotB = pivotB;

					ret = true;
				}
			}
		}
	}

	UpdateParams();

	EndRefresh();

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

	return ret;
}

const Mix::Tool::Win32::Dynamics::RigidBody* PointJoint::GetRigidBodyA( void ) const
{
	return m_pRigidBodyA;
}

D3DXVECTOR3 PointJoint::GetPivotA( void )
{
	if( m_pConstraint == NULL )
	{
		return D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	}

	const btVector3& pivot = m_pConstraint->getPivotInA();

	return D3DXVECTOR3( pivot.x(), pivot.y(), pivot.z() );
}

void PointJoint::SetPivotA( const D3DXVECTOR3& pivot )
{
	if( m_pConstraint != NULL )
	{
		m_pConstraint->setPivotA( btVector3( pivot.x, pivot.y, pivot.z ) );
	}

	m_PivotA = pivot;
}

const Mix::Tool::Win32::Dynamics::RigidBody* PointJoint::GetRigidBodyB( void ) const
{
	return m_pRigidBodyB;
}

D3DXVECTOR3 PointJoint::GetPivotB( void )
{
	if( m_pConstraint == NULL )
	{
		return D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	}

	const btVector3& pivot = m_pConstraint->getPivotInB();

	return D3DXVECTOR3( pivot.x(), pivot.y(), pivot.z() );
}

void PointJoint::SetPivotB( const D3DXVECTOR3& pivot )
{
	if( m_pConstraint != NULL )
	{
		m_pConstraint->setPivotB( btVector3( pivot.x, pivot.y, pivot.z ) );
	}

	m_PivotB = pivot;
}

bool PointJoint::GetCollisionDisabled( void ) const
{
	return m_bCollisionDisabled;
}

void PointJoint::SetCollisionDisabled( bool state )
{
	m_bCollisionDisabled = state;

	Refresh();

	if( m_pRigidBodyA != NULL ) { m_pRigidBodyA->Activate(); }
	if( m_pRigidBodyB != NULL ) { m_pRigidBodyB->Activate(); }
}

bool PointJoint::IsParamSupported( int type, int axis ) const
{
	if( ( ( type == PointJoint::PT_ERP ) || ( type == PointJoint::PT_CFM ) ) &&
		( axis == PointJoint::PA_NONE ) )
	{
		return true;
	}

	return false;
}

bool PointJoint::IsParamEnabled( int type, int axis ) const
{
	if( IsParamSupported( type, axis ) == false )
	{
		return false;
	}

	unsigned int bit = ( 1 << type );

	return ( MIX_TEST_BIT( m_ParamFlags, bit ) == bit );
}

float PointJoint::GetParam( int type, int axis ) const
{
	if( IsParamSupported( type, axis ) == false )
	{
		return 0.0f;
	}

	return m_ParamValues[type];
}

void PointJoint::SetParam( int type, int axis, float value )
{
	if( IsParamSupported( type, axis ) == true )
	{
		MIX_SET_BIT( m_ParamFlags, ( 1 << type ) );
		m_ParamValues[type] = value;

		if( m_pConstraint != NULL )
		{
			m_pConstraint->setParam( type, value, axis );
		}
	}
}

void PointJoint::ResetParam( int type, int axis )
{
	if( IsParamSupported( type, axis ) == true )
	{
		unsigned int bit = 1 << type;

		if( MIX_TEST_BIT( m_ParamFlags, bit ) == bit )
		{
			MIX_RESET_BIT( m_ParamFlags, bit );
			m_ParamValues[type] = 0.0f;

			if( m_pConstraint != NULL )
			{
				if( ( m_pRigidBodyA != NULL ) &&
					( m_pRigidBodyB == NULL ) )
				{
					SetFrame( m_pRigidBodyA, m_PivotA );
				}
				else if(	( m_pRigidBodyA != NULL ) &&
							( m_pRigidBodyB != NULL ) )
				{
					SetFrame( m_pRigidBodyA, m_pRigidBodyB, m_PivotA, m_PivotB );
				}
			}
		}
	}
}

void PointJoint::Draw(	Mix::Tool::Win32::Graphics::LineHelper* pLineHelper,
						const D3DXVECTOR4& axisColor,
						const D3DXVECTOR4& ballColor,
						const D3DXVECTOR4& limitColor,
						float scale )
{
	if( ( pLineHelper == NULL ) ||
		( m_pConstraint == NULL ) )
	{
		return;
	}

	const btVector3& pivotA = m_pConstraint->getPivotInA();
	const btVector3& pivotB = m_pConstraint->getPivotInB();
	const btRigidBody* pRigidBodyA = &( m_pConstraint->getRigidBodyA() );
	const btRigidBody* pRigidBodyB = &( m_pConstraint->getRigidBodyB() );

	D3DXVECTOR3 pointA;
	D3DXVECTOR3 pointB;

	Mix::Tool::Win32::Geometry::SPHERE sphere;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 初期化
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pLineHelper->IdentityMatrix();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 軸
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pLineHelper->SetColor( axisColor );

	if( pRigidBodyA != &( btTypedConstraint::getFixedBody() ) )
	{
		btTransform world;
		btQuaternion rot;
		btVector3 pos;
		D3DXMATRIX mat;

		if( MIX_TEST_BIT( pRigidBodyA->getCollisionFlags(), btCollisionObject::CF_KINEMATIC_OBJECT ) == btCollisionObject::CF_KINEMATIC_OBJECT )
		{
			pRigidBodyA->getMotionState()->getWorldTransform( world );
		}
		else
		{
			world = pRigidBodyA->getWorldTransform();
		}

		rot = world.getRotation();
		pos = world.getOrigin();

		D3DXMatrixRotationQuaternion( &mat, &D3DXQUATERNION( rot.x(), rot.y(), rot.z(), rot.w() ) );
		mat._41 = pos.x(); mat._42 = pos.y(); mat._43 = pos.z(); mat._44 = 1.0f;

		D3DXVec3TransformCoord( &pointA, &D3DXVECTOR3( pivotA.x(), pivotA.y(), pivotA.z() ), &mat );

		pLineHelper->AddPoints( pointA, D3DXVECTOR3( pos.x(), pos.y(), pos.z() ) );
	}
	else
	{
		pointA = D3DXVECTOR3( pivotA.x(), pivotA.y(), pivotA.z() );
	}

	if( pRigidBodyB != &( btTypedConstraint::getFixedBody() ) )
	{
		btTransform world;
		btQuaternion rot;
		btVector3 pos;
		D3DXMATRIX mat;
		D3DXVECTOR3 point;

		if( MIX_TEST_BIT( pRigidBodyB->getCollisionFlags(), btCollisionObject::CF_KINEMATIC_OBJECT ) == btCollisionObject::CF_KINEMATIC_OBJECT )
		{
			pRigidBodyB->getMotionState()->getWorldTransform( world );
		}
		else
		{
			world = pRigidBodyB->getWorldTransform();
		}

		rot = world.getRotation();
		pos = world.getOrigin();

		D3DXMatrixRotationQuaternion( &mat, &D3DXQUATERNION( rot.x(), rot.y(), rot.z(), rot.w() ) );
		mat._41 = pos.x(); mat._42 = pos.y(); mat._43 = pos.z(); mat._44 = 1.0f;

		D3DXVec3TransformCoord( &pointB, &D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() ), &mat );

		pLineHelper->AddPoints( pointB, D3DXVECTOR3( pos.x(), pos.y(), pos.z() ) );
	}
	else
	{
		pointB = D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ボール
	////////////////////////////////////////////////////////////////////////////////////////////////////

	sphere.pos = ( pointA + pointB ) * 0.5f;
	sphere.radius = max( Mix::Tool::Win32::Dynamics::Impl::CONSTRAINT_MIN_JOINT_RADIUS, D3DXVec3Length( &( pointA - pointB ) ) * 0.5f );

	pLineHelper->SetColor( ballColor );
	pLineHelper->AddSphere( sphere );
}

void* PointJoint::Bullet_GetTypedConstraintPtr( void )
{
	return m_pConstraint;
}

void PointJoint::BeginRefresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->DetachJoint( this, false );
	}
}

void PointJoint::EndRefresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->AttachJoint( this, false );
	}
}

void PointJoint::Refresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->DetachJoint( this, false );
		m_pWorld->AttachJoint( this, false );
	}
}

void PointJoint::UpdateParams( void )
{
	if( m_pConstraint != NULL )
	{
		for( unsigned int i = 0; i < PointJoint::PT_MAX; i++ )
		{
			unsigned int bit = ( 1 << i );

			if( MIX_TEST_BIT( m_ParamFlags, bit ) == bit )
			{
				m_pConstraint->setParam( i, m_ParamValues[i] );
			}
		}
	}
}

}}}}}
