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

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

SliderJoint* SliderJoint::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, SliderJoint, pRigidBodyA, pRigidBodyB, rotA, rotB, pivotA, pivotB );
}

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

SliderJoint::SliderJoint(	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_pObject( NULL ),
m_pRigidBodyA( pRigidBodyA ),
m_pRigidBodyB( pRigidBodyB ),
m_RotA( rotA ),
m_RotB( rotB ),
m_PivotA( pivotA ),
m_PivotB( pivotB ),
m_ParamFlags( 0 )
{
	MIX_ADD_REF( m_pRigidBodyA );
	MIX_ADD_REF( m_pRigidBodyB );
}

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

	MIX_RELEASE( m_pRigidBodyA );
	MIX_RELEASE( m_pRigidBodyB );
}

Boolean SliderJoint::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 btSliderConstraint( *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 btSliderConstraint( *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]", SliderJoint::FAILED_CREATE, Mix::STR_OUTOFMEMORY, pDebugName );
		return False;
	}

	m_pObject->buildJacobian();

	return True;
}

void SliderJoint::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 SliderJoint::GetType( void ) const
{
	return Mix::Dynamics::IJoint::SLIDER;
}

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

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

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

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

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

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

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

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

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

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

	UpdateFrames();
}

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

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

	UpdateFrames();
}

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

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

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

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

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

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

void SliderJoint::Debug_Draw( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer, Float32 opacity )
{
	const btTransform& frameA = m_pObject->getFrameOffsetA();
	const btTransform& frameB = m_pObject->getFrameOffsetB();
	const btVector3& pivotA = frameA.getOrigin();
	const btVector3& pivotB = frameB.getOrigin();
	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::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 )
	{
		btTransform tr = m_pObject->getUseLinearReferenceFrameA() ? m_pObject->getCalculatedTransformA() : m_pObject->getCalculatedTransformB();
		btVector3 li_min = tr * btVector3( m_pObject->getLowerLinLimit(), 0.0f, 0.0f );
		btVector3 li_max = tr * btVector3( m_pObject->getUpperLinLimit(), 0.0f, 0.0f );
		btVector3 normal = tr.getBasis().getColumn(0);
		btVector3 axis = tr.getBasis().getColumn(1);
		btScalar minAngle = m_pObject->getLowerAngLimit();
		btScalar maxAngle = m_pObject->getUpperAngLimit();
		const btVector3& center = m_pObject->getCalculatedTransformB().getOrigin();

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

		pPerspectiveRenderer->AddLine( ToMixVector3( li_min ), ToMixVector3( li_max ) );

		Debug::DrawArc( pPerspectiveRenderer,
						center,
						normal,
						axis,
						m_DebugDrawLimitScaling,
						m_DebugDrawLimitScaling,
						minAngle,
						maxAngle );
	}

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

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

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

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

	UpdateFrames();
}

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

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

	UpdateFrames();
}

void SliderJoint::SetLinearLimit( Float32 lower, Float32 upper )
{
	m_pObject->setLowerLinLimit( lower );
	m_pObject->setUpperLinLimit( upper );
}

Float32 SliderJoint::GetLinearLowerLimit( void ) const
{
	return m_pObject->getLowerLinLimit();
}

Float32 SliderJoint::GetLinearUpperLimit( void ) const
{
	return m_pObject->getUpperLinLimit();
}

void SliderJoint::SetAngularLimit( Float32 lower, Float32 upper )
{
	m_pObject->setLowerAngLimit( lower );
	m_pObject->setUpperAngLimit( upper );
}

Float32 SliderJoint::GetAngularLowerLimit( void ) const
{
	return m_pObject->getLowerAngLimit();
}

Float32 SliderJoint::GetAngularUpperLimit( void ) const
{
	return m_pObject->getUpperAngLimit();
}

Boolean SliderJoint::GetLinearMotorEnabled( void ) const
{
	return m_pObject->getPoweredLinMotor();
}

void SliderJoint::SetLinearMotorEnabled( Boolean state )
{
	m_pObject->setPoweredLinMotor( ( state == True ) );
}

Float32 SliderJoint::GetLinearMotorTargetVelocity( void ) const
{
	return m_pObject->getTargetLinMotorVelocity();
}

void SliderJoint::SetLinearMotorTargetVelocity( Float32 tv )
{
	m_pObject->setTargetLinMotorVelocity( tv );
}

Float32 SliderJoint::GetLinearMotorMaxForce( void ) const
{
	return m_pObject->getMaxLinMotorForce();
}

void SliderJoint::SetLinearMotorMaxForce( Float32 mf )
{
	m_pObject->setMaxLinMotorForce( mf );
}

Boolean SliderJoint::GetAngularMotorEnabled( void ) const
{
	return m_pObject->getPoweredAngMotor();
}

void SliderJoint::SetAngularMotorEnabled( Boolean state )
{
	m_pObject->setPoweredAngMotor( ( state == True ) );
}

Float32 SliderJoint::GetAngularMotorTargetVelocity( void ) const
{
	return m_pObject->getTargetAngMotorVelocity();
}

void SliderJoint::SetAngularMotorTargetVelocity( Float32 tv )
{
	m_pObject->setTargetAngMotorVelocity( tv );
}

Float32 SliderJoint::GetAngularMotorMaxForce( void ) const
{
	return m_pObject->getMaxAngMotorForce();
}

void SliderJoint::SetAngularMotorMaxForce( Float32 mf )
{
	m_pObject->setMaxAngMotorForce( mf );
}

Float32 SliderJoint::GetLinearLimit_LimSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_LL_LS ) != SliderJoint::PF_LL_LS )
	{
		return 0.0f;
	}

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

void SliderJoint::SetLinearLimit_LimSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, spring, 0 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_LL_LS );
}

Float32 SliderJoint::GetLinearLimit_LimDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_LL_LD ) != SliderJoint::PF_LL_LD )
	{
		return 0.0f;
	}

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

void SliderJoint::SetLinearLimit_LimDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, damper, 0 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_LL_LD );
}

Float32 SliderJoint::GetLinearLimit_OrthoSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_LL_OS ) != SliderJoint::PF_LL_OS )
	{
		return 0.0f;
	}

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

void SliderJoint::SetLinearLimit_OrthoSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, spring, 1 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_LL_OS );
}

Float32 SliderJoint::GetLinearLimit_OrthoDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_LL_OD ) != SliderJoint::PF_LL_OD )
	{
		return 0.0f;
	}

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

void SliderJoint::SetLinearLimit_OrthoDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, damper, 1 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_LL_OD );
}


Float32 SliderJoint::GetAngularLimit_LimSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_AL_LS ) != SliderJoint::PF_AL_LS )
	{
		return 0.0f;
	}

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

void SliderJoint::SetAngularLimit_LimSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, spring, 3 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_AL_LS );
}

Float32 SliderJoint::GetAngularLimit_LimDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_AL_LD ) != SliderJoint::PF_AL_LD )
	{
		return 0.0f;
	}

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

void SliderJoint::SetAngularLimit_LimDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, damper, 3 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_AL_LD );
}

Float32 SliderJoint::GetAngularLimit_OrthoSpring( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_AL_OS ) != SliderJoint::PF_AL_OS )
	{
		return 0.0f;
	}

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

void SliderJoint::SetAngularLimit_OrthoSpring( Float32 spring )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_CFM, spring, 4 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_AL_OS );
}

Float32 SliderJoint::GetAngularLimit_OrthoDamper( void ) const
{
	if( MIX_TESTBIT( m_ParamFlags, SliderJoint::PF_AL_OD ) != SliderJoint::PF_AL_OD )
	{
		return 0.0f;
	}

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

void SliderJoint::SetAngularLimit_OrthoDamper( Float32 damper )
{
	m_pObject->setParam( BT_CONSTRAINT_STOP_ERP, damper, 4 );

	MIX_SETBIT( m_ParamFlags, SliderJoint::PF_AL_OD );
}

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

}}
