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

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

#include "btBulletDynamicsCommon.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletDynamics/Character/btKinematicCharacterController.h"

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

const float KinematicCharacter::DEF_STEPOFFSET = 0.3f;
const float KinematicCharacter::MIN_LINEAR_VELOCITY_Y = 0.0001f;

KinematicCharacter::KinematicCharacter( void ) :
m_pWorld( NULL ),
m_pShape( NULL ),
m_pGhostObject( NULL ),
m_pCharacterController( NULL ),
m_Group( FG_CHARACTER ),
m_GroupMask( FG_STATIC | FG_KINEMATIC | FG_SENSOR | FG_CHARACTER )
{
	m_Radius = KinematicCharacter::DefaultRadius();
	m_Height = KinematicCharacter::DefaultHeight();
	m_Gravity = 29.4f;
	m_MaxFallSpeed = 58.8f;
	m_InitJumpSpeed = 9.8f;
	m_StepOffset = 0.3f;
	m_SlopeLimit = 45.0f;
	m_LinearVelocity = D3DXVECTOR3( 0.0f, KinematicCharacter::MIN_LINEAR_VELOCITY_Y, 0.0f );

	D3DXQuaternionIdentity( &m_Rot );
	m_Basis = btMatrix3x3::getIdentity();
}

KinematicCharacter::~KinematicCharacter( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->RemoveCollisionObject( this );
	}

	MIX_DELETE( m_pCharacterController );
	MIX_DELETE( m_pGhostObject );
	MIX_DELETE( m_pShape );
}

bool KinematicCharacter::Initialize( void )
{
	return Refresh();
}

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

btPairCachingGhostObject* KinematicCharacter::Bullet_GetGhostObjectPtr( void )
{
	return m_pGhostObject;
}

btKinematicCharacterController* KinematicCharacter::Bullet_GetCharacterControllerPtr( void )
{
	return m_pCharacterController;
}

const Mix::Tool::Win32::Dynamics::MATERIAL& KinematicCharacter::GetMaterial( void ) const
{
	return m_Material;
}

void KinematicCharacter::SetMaterial( const Mix::Tool::Win32::Dynamics::MATERIAL& material )
{
	m_Material = material;

	if( m_pGhostObject != NULL )
	{
		m_pGhostObject->setFriction( m_Material.friction );
		m_pGhostObject->setRestitution( m_Material.restitution );
	}
}

float KinematicCharacter::GetHeight( void ) const
{
	return m_Height;
}

void KinematicCharacter::SetHeight( float height )
{
	m_Height = max( 0.00001f, height );

	Refresh();
}

float KinematicCharacter::GetRadius( void ) const
{
	return m_Radius;
}

void KinematicCharacter::SetRadius( float radius )
{
	m_Radius = max( 0.00001f, radius );

	Refresh();
}

float KinematicCharacter::GetGravity( void ) const
{
	return m_Gravity;
}

void KinematicCharacter::SetGravity( float gravity )
{
	m_Gravity = max( 0.0f, gravity );

	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->setGravity( m_Gravity );
	}
}

float KinematicCharacter::GetMaxFallSpeed( void ) const
{
	return m_MaxFallSpeed;
}

void KinematicCharacter::SetMaxFallSpeed( float speed )
{
	m_MaxFallSpeed = max( 0.0f, speed );

	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->setFallSpeed( m_MaxFallSpeed );
	}
}

float KinematicCharacter::GetInitJumpSpeed( void ) const
{
	return m_InitJumpSpeed;
}

void KinematicCharacter::SetInitJumpSpeed( float speed )
{
	m_InitJumpSpeed = max( 0.0f, speed );

	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->setJumpSpeed( m_InitJumpSpeed );
	}
}

float KinematicCharacter::GetStepOffset( void ) const
{
	return m_StepOffset;
}

void KinematicCharacter::SetStepOffset( float stepOffset )
{
	m_StepOffset = max( 0.0f, stepOffset );

	Refresh();
}

float KinematicCharacter::GetSlopeLimit( void ) const
{
	return m_SlopeLimit;
}

void KinematicCharacter::SetSlopeLimit( float slopeLimit )
{
	m_SlopeLimit = MIX_CLAMP( slopeLimit, 0.0f, 90.0f );

	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->setMaxSlope( D3DXToRadian( m_SlopeLimit ) );
	}
}

const D3DXVECTOR3& KinematicCharacter::GetLinearVelocity( void ) const
{
	return m_LinearVelocity;
}

void KinematicCharacter::SetLinearVelocity( const D3DXVECTOR3& vel )
{
	m_LinearVelocity = vel;

	if( m_pCharacterController != NULL )
	{
		btVector3 walkDir;

		if( D3DXVec3Length( &vel ) >= MIN_LINEAR_VELOCITY_Y )
		{
			walkDir = m_Basis * btVector3( vel.x, vel.y, vel.z );
		}
		else
		{
			walkDir.setValue( 0.0f, -MIN_LINEAR_VELOCITY_Y, 0.0f );
		}

		m_pCharacterController->setWalkDirection( walkDir );
	}
}

bool KinematicCharacter::IsGround( void ) const
{
	return m_pCharacterController->onGround();
}

void KinematicCharacter::Jump( void )
{
	if( IsInWorld() == true )
	{
		m_pCharacterController->jump();
	}
}

short KinematicCharacter::GetGroup( void ) const
{
	return m_Group;
}

void KinematicCharacter::SetGroup( short data )
{
	BeginRefresh();
	m_Group = data;
	EndRefresh();
}

short KinematicCharacter::GetGroupMask( void ) const
{
	return m_GroupMask;
}

void KinematicCharacter::SetGroupMask( short data )
{
	BeginRefresh();
	m_GroupMask = data;
	EndRefresh();
}

D3DXQUATERNION KinematicCharacter::GetWorldRotation( void ) const
{
	return m_Rot;
}

void KinematicCharacter::SetWorldRotation( const D3DXQUATERNION& rot )
{
	m_Rot = rot;
	m_Basis.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
}

D3DXVECTOR3 KinematicCharacter::GetWorldPosition( void ) const
{
	D3DXVECTOR3 ret;

	if( m_pGhostObject != NULL )
	{
		const btVector3& pos = m_pGhostObject->getWorldTransform().getOrigin();

		ret.x = pos.x();
		ret.y = pos.y();
		ret.z = pos.z();
	}
	else
	{
		ret.x = 0.0f;
		ret.y = 0.0f;
		ret.z = 0.0f;
	}

	return ret;
}

void KinematicCharacter::SetWorldPosition( const D3DXVECTOR3& pos )
{
	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->warp( btVector3( pos.x, pos.y, pos.z ) );
	}
}

D3DXMATRIX KinematicCharacter::GetWorldMatrix( void ) const
{
	D3DXQUATERNION rot = GetWorldRotation();
	D3DXVECTOR3 pos = GetWorldPosition();
	D3DXMATRIX mat;

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

	return mat;
}

void KinematicCharacter::SetWorldMatrix( const D3DXMATRIX& mat )
{
	D3DXQUATERNION rot;
	D3DXVECTOR3 pos;

	Mix::Tool::Win32::Matrix_Decompose( mat, NULL, &rot, &pos );

	SetWorldTransform( rot, pos );
}

void KinematicCharacter::SetWorldTransform( const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	SetWorldRotation( rot );
	SetWorldPosition( pos );
}

bool KinematicCharacter::IsInWorld( void ) const
{
	return ( m_pWorld != NULL );
}

void KinematicCharacter::Draw( Mix::Tool::Win32::Graphics::LineHelper* pLineHelper, const D3DXVECTOR4& color )
{
	if( ( pLineHelper == NULL ) ||
		( m_pShape == NULL ) ||
		( m_pGhostObject == NULL ) )
	{
		return;
	}

	const btVector3& worldPos = m_pGhostObject->getWorldTransform().getOrigin();
	D3DXMATRIX worldMat;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールド行列の作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixIdentity( &worldMat );
	D3DXMatrixTranslation( &worldMat, worldPos.x(), worldPos.y(), worldPos.z() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// シェイプ
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Mix::Tool::Win32::Geometry::CAPSULE capsule;

	switch( m_pShape->getUpAxis() )
	{
	case 0:
		capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_X;
		break;
	case 1:
		capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_Y;
		break;
	case 2:
		capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_Z;
		break;
	}

	capsule.pos = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	capsule.halfLen = m_pShape->getHalfHeight();
	capsule.radius = m_pShape->getRadius();

	pLineHelper->SetColor( color );
	pLineHelper->SetMatrix( worldMat );
	pLineHelper->AddCapsule( capsule );
}

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

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

bool KinematicCharacter::Refresh( void )
{
	float axisLength = max( 0.0f, m_Height - ( m_Radius * 2.0f ) );
//	btVector3 pos( m_Center.x, m_Center.y, m_Center.z );
	btVector3 pos( 0.0f, 0.0f, 0.0f );

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

	BeginRefresh();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 直前のものを破棄
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_pGhostObject != NULL )
	{
		pos = m_pGhostObject->getWorldTransform().getOrigin();
	}

	MIX_DELETE( m_pCharacterController );
	MIX_DELETE( m_pGhostObject );
	MIX_DELETE( m_pShape );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// シェイプの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pShape = new btCapsuleShape( m_Radius, axisLength );
	if( m_pShape != NULL )
	{
		m_pShape->setUserPointer( &m_Material );
	}
	else
	{
		return false;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ゴーストオブジェクトの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pGhostObject = new btPairCachingGhostObject();
	if( m_pGhostObject != NULL )
	{
		m_pGhostObject->setFriction( m_Material.friction );
		m_pGhostObject->setRestitution( m_Material.restitution );
		m_pGhostObject->setCollisionShape( m_pShape );
		m_pGhostObject->setCollisionFlags( btCollisionObject::CF_CHARACTER_OBJECT );
		m_pGhostObject->setUserPointer( this );
	}
	else
	{
		MIX_DELETE( m_pShape );
		return false;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// キャラクターコントローラーの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pCharacterController = new btKinematicCharacterController( m_pGhostObject, m_pShape, m_StepOffset );
	if( m_pCharacterController != NULL )
	{
		m_pCharacterController->setUseGhostSweepTest( false );
		m_pCharacterController->setUpInterpolate( true );
		m_pCharacterController->setGravity( m_Gravity );
		m_pCharacterController->setFallSpeed( m_MaxFallSpeed );
		m_pCharacterController->setJumpSpeed( m_InitJumpSpeed );
		m_pCharacterController->setMaxSlope( m_SlopeLimit );
		m_pCharacterController->warp( pos );

		SetLinearVelocity( m_LinearVelocity );
	}
	else
	{
		MIX_DELETE( m_pGhostObject );
		MIX_DELETE( m_pShape );
		return false;
	}

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

	EndRefresh();

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

	return true;
}

}}}}}
