#include "Mix/Private/Dynamics/KinematicCharacter.h"

#include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "LinearMath/btDefaultMotionState.h"

#include "Mix/Graphics/Utility/IPerspectiveRenderer.h"
#include "Mix/Private/Dynamics/Utility.h"
#include "Mix/Private/Dynamics/CapsuleShape.h"
#include "Mix/Private/Dynamics/ObjectContext.h"

namespace Mix{ namespace Dynamics{

const wchar_t* KinematicCharacter::FAILED_CREATE = L"キネマティックキャラクターの作成に失敗";
const Float32 KinematicCharacter::MIN_LINEAR_VELOCITY = 0.0001f; //最低移動速度( 0.0f を指定されると落下時にアサートだしてぶっ飛ばされる対策 )

////////////////////////////////////////////////////////////////////////////////////////////////////
// KinematicCharacter::ClosestNotMeRayResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

KinematicCharacter::ClosestNotMeRayResultCallback::ClosestNotMeRayResultCallback( btCollisionObject* me ) :
btCollisionWorld::ClosestRayResultCallback( btVector3( 0.0, 0.0, 0.0 ), btVector3( 0.0, 0.0, 0.0 ) )
{
	m_me = me;
}

btScalar KinematicCharacter::ClosestNotMeRayResultCallback::addSingleResult( btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace )
{
	if( rayResult.m_collisionObject == m_me )
	{
		return 1.0f;
	}

	return ClosestRayResultCallback::addSingleResult( rayResult, normalInWorldSpace );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// KinematicCharacter::ClosestNotMeConvexResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

KinematicCharacter::ClosestNotMeConvexResultCallback::ClosestNotMeConvexResultCallback( btCollisionObject* me, const btVector3& up, btScalar minSlopeDot )
: btCollisionWorld::ClosestConvexResultCallback( btVector3( 0.0, 0.0, 0.0 ), btVector3( 0.0, 0.0, 0.0 ) ),
m_me( me ),
m_up( up ),
m_minSlopeDot( minSlopeDot )
{
}

btScalar KinematicCharacter::ClosestNotMeConvexResultCallback::addSingleResult( btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace )
{
	if( convexResult.m_hitCollisionObject == m_me )
	{
		return btScalar( 1.0f );
	}

	if( !convexResult.m_hitCollisionObject->hasContactResponse() )
	{
		return btScalar( 1.0f );
	}

	btVector3 hitNormalWorld;

	if( normalInWorldSpace )
	{
		hitNormalWorld = convexResult.m_hitNormalLocal;
	}
	else
	{
		///need to transform normal into worldspace
		hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal;
	}

	btScalar dotUp = m_up.dot(hitNormalWorld);

	if( dotUp < m_minSlopeDot )
	{
		return btScalar( 1.0f );
	}

	return ClosestConvexResultCallback::addSingleResult( convexResult, normalInWorldSpace );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// KinematicCharacter::Controller
////////////////////////////////////////////////////////////////////////////////////////////////////

/*
	Controller
*/

KinematicCharacter::Controller::Controller( btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, int upAxis )
{
	m_upAxis = upAxis;
	m_addedMargin = btScalar( 0.02 );
	m_walkDirection.setValue( btScalar( 0.0 ), btScalar( 0.0 ), btScalar( 0.0 ) );
	m_useGhostObjectSweepTest = true;
	m_ghostObject = ghostObject;
	m_stepHeight = stepHeight;
	m_turnAngle = btScalar( 0.0 );
	m_convexShape=convexShape;	
	m_useWalkDirection = true;	// use walk direction by default, legacy behavior
	m_velocityTimeInterval = btScalar( 0.0 );
	m_verticalVelocity = btScalar( 0.0 );
	m_verticalOffset = btScalar( 0.0 );
	m_gravity = btScalar( 9.8 ) * btScalar( 3.0 ); // 3G acceleration.
	m_fallSpeed = btScalar( 55.0 ); // Terminal velocity of a sky diver in m/s.
	m_jumpSpeed = btScalar( 10.0 ); // ?
	m_wasOnGround = false;
	m_wasJumping = false;
	m_interpolateUp = true;
	setMaxSlope( btRadians( 45.0 ) );
	m_currentStepOffset = btScalar( 0.0 );

	m_activeThreshold = btScalar( 0.00012 );
	m_activeThreshold2 = m_activeThreshold * m_activeThreshold;
	m_deactivationElapsedTime = btScalar( 1.0 );
	m_deactiveTimeCounter = btScalar( 0.0 );
	m_active = true;

	full_drop = false;
	bounce_fix = false;
}

KinematicCharacter::Controller::~Controller( void )
{
}

void KinematicCharacter::Controller::setUpAxis( int axis )
{
	if( axis < 0 )
	{
		axis = 0;
	}
	else if( axis > 2 )
	{
		axis = 2;
	}

	m_upAxis = axis;
}

void KinematicCharacter::Controller::setFallSpeed( btScalar fallSpeed )
{
	m_fallSpeed = fallSpeed;
}

void KinematicCharacter::Controller::setJumpSpeed( btScalar jumpSpeed )
{
	m_jumpSpeed = jumpSpeed;
}

void KinematicCharacter::Controller::setMaxJumpHeight( btScalar maxJumpHeight )
{
	m_maxJumpHeight = maxJumpHeight;
}

btScalar KinematicCharacter::Controller::getGravity( void ) const
{
	return m_gravity;
}

void KinematicCharacter::Controller::setGravity( btScalar gravity )
{
	m_gravity = gravity;
}

btScalar KinematicCharacter::Controller::getMaxSlope() const
{
	return m_maxSlopeRadians;
}

void KinematicCharacter::Controller::setMaxSlope( btScalar slopeRadians )
{
	m_maxSlopeRadians = slopeRadians;
	m_maxSlopeCosine = btCos( slopeRadians );
}

btScalar KinematicCharacter::Controller::getActiveThreshold( void ) const
{
	return m_activeThreshold;
}

void KinematicCharacter::Controller::setActiveThreshold( btScalar threshold )
{
	btAssert( threshold > btScalar( 0.0 ) );

	m_activeThreshold = threshold;
	m_activeThreshold2 = threshold * threshold;
}

btScalar KinematicCharacter::Controller::getDeactivationElapsedTime( void ) const
{
	return m_deactivationElapsedTime;
}

void KinematicCharacter::Controller::setDeactivationElapsedTime( btScalar elapsedTime )
{
	btAssert( elapsedTime > btScalar( 0.0 ) );

	m_deactivationElapsedTime = elapsedTime;
}

bool KinematicCharacter::Controller::isActive( void ) const
{
	return m_active;
}

btPairCachingGhostObject* KinematicCharacter::Controller::getGhostObject( void )
{
	return m_ghostObject;
}

void KinematicCharacter::Controller::setUseGhostSweepTest( bool useGhostObjectSweepTest )
{
	m_useGhostObjectSweepTest = useGhostObjectSweepTest;
}

/*
 * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
 *
 * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
*/
btVector3 KinematicCharacter::Controller::computeReflectionDirection( const btVector3& direction, const btVector3& normal )
{
	return direction - ( btScalar( 2.0 ) * direction.dot( normal ) ) * normal;
}

/*
 * Returns the portion of 'direction' that is parallel to 'normal'
*/
btVector3 KinematicCharacter::Controller::parallelComponent( const btVector3& direction, const btVector3& normal )
{
	btScalar magnitude = direction.dot( normal );
	return normal * magnitude;
}

/*
 * Returns the portion of 'direction' that is perpindicular to 'normal'
 */
btVector3 KinematicCharacter::Controller::perpindicularComponent( const btVector3& direction, const btVector3& normal )
{
	return direction - parallelComponent( direction, normal );
}

bool KinematicCharacter::Controller::recoverFromPenetration( btCollisionWorld* collisionWorld )
{
	// Here we must refresh the overlapping paircache as the penetrating movement itself or the
	// previous recovery iteration might have used setWorldTransform and pushed us into an object
	// that is not in the previous cache contents from the last timestep, as will happen if we
	// are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck.
	//
	// Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase
	// paircache and the ghostobject's internal paircache at the same time.    /BW

	btVector3 minAabb;
	btVector3 maxAabb;
	bool penetration = false;

	m_convexShape->getAabb( m_ghostObject->getWorldTransform(), minAabb, maxAabb );

	collisionWorld->getBroadphase()->setAabb( m_ghostObject->getBroadphaseHandle(),
		minAabb, maxAabb,
		collisionWorld->getDispatcher() );

	collisionWorld->getDispatcher()->dispatchAllCollisionPairs( m_ghostObject->getOverlappingPairCache(),
		collisionWorld->getDispatchInfo(),
		collisionWorld->getDispatcher() );

	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
	
	btScalar maxPen = btScalar( 0.0 );

	for( int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++ )
	{
		m_manifoldArray.resize( 0 );

		btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];

		btCollisionObject* obj0 = static_cast<btCollisionObject*>( collisionPair->m_pProxy0->m_clientObject );
		btCollisionObject* obj1 = static_cast<btCollisionObject*>( collisionPair->m_pProxy1->m_clientObject );

		if( ( obj0 && !obj0->hasContactResponse() ) || ( obj1 && !obj1->hasContactResponse() ) )
		{
			continue;
		}
		
		if( collisionPair->m_algorithm )
		{
			collisionPair->m_algorithm->getAllContactManifolds( m_manifoldArray );
		}
		
		for( int j = 0; j < m_manifoldArray.size(); j++ )
		{
			btPersistentManifold* manifold = m_manifoldArray[j];
			btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar( -1.0 ) : btScalar( 1.0 );

			for( int p = 0; p < manifold->getNumContacts(); p++)
			{
				const btManifoldPoint& pt = manifold->getContactPoint( p );

				btScalar dist = pt.getDistance();

				if( dist < 0.0 )
				{
					if( dist < maxPen )
					{
						maxPen = dist;
						m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
					}

					m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar( 0.2 );
					penetration = true;
				}
				else
				{
					//printf("touching %f\n", dist);
				}
			}
			
			//manifold->clearManifold();
		}
	}

	btTransform newTrans = m_ghostObject->getWorldTransform();

	newTrans.setOrigin(m_currentPosition);
	m_ghostObject->setWorldTransform(newTrans);

	return penetration;
}

void KinematicCharacter::Controller::stepUp( btCollisionWorld* world )
{
	// phase 1: up
	btTransform start;
	btTransform end;

	m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * ( m_stepHeight + ( ( m_verticalOffset > 0.0f )? m_verticalOffset : 0.0f ) );

	start.setIdentity();
	end.setIdentity();

	/* FIXME: Handle penetration properly */
	start.setOrigin(m_currentPosition + getUpAxisDirections()[m_upAxis] * ( m_convexShape->getMargin() + m_addedMargin ) );
	end.setOrigin( m_targetPosition );

	KinematicCharacter::ClosestNotMeConvexResultCallback callback( m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar( 0.7071 ) );
	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
	
	if( m_useGhostObjectSweepTest )
	{
		m_ghostObject->convexSweepTest( m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration );
	}
	else
	{
		world->convexSweepTest( m_convexShape, start, end, callback );
	}
	
	if( callback.hasHit() )
	{
		// Only modify the position if the hit was a slope and not a wall or ceiling.
		if( callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0 )
		{
			// we moved up only a fraction of the step height
			m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
			if( m_interpolateUp == true )
			{
				m_currentPosition.setInterpolate3( m_currentPosition, m_targetPosition, callback.m_closestHitFraction );
			}
			else
			{
				m_currentPosition = m_targetPosition;
			}
		}

		m_verticalVelocity = 0.0;
		m_verticalOffset = 0.0;

	}
	else
	{
		m_currentStepOffset = m_stepHeight;
		m_currentPosition = m_targetPosition;
	}
}

void KinematicCharacter::Controller::updateTargetPositionBasedOnCollision( const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag )
{
	btVector3 movementDirection = m_targetPosition - m_currentPosition;
	btScalar movementLength = movementDirection.length();

	if( movementLength > SIMD_EPSILON )
	{
		movementDirection.normalize();

		btVector3 reflectDir = computeReflectionDirection( movementDirection, hitNormal );
		reflectDir.normalize();

		btVector3 parallelDir, perpindicularDir;

		parallelDir = parallelComponent( reflectDir, hitNormal );
		perpindicularDir = perpindicularComponent( reflectDir, hitNormal );

		m_targetPosition = m_currentPosition;

#if 0
		if( 0 )//tangentMag != 0.0)
		{
			btVector3 parComponent = parallelDir * btScalar ( tangentMag*movementLength );
//			printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
			m_targetPosition +=  parComponent;
		}
#endif

		if( normalMag != 0.0 )
		{
			btVector3 perpComponent = perpindicularDir * btScalar( normalMag * movementLength );
//			printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
			m_targetPosition += perpComponent;
		}
	}
	else
	{
//		printf("movementLength don't normalize a zero vector\n");
	}
}

void KinematicCharacter::Controller::stepForwardAndStrafe( btCollisionWorld* collisionWorld, const btVector3& walkMove )
{
	// printf("m_normalizedDirection=%f,%f,%f\n",
	// 	m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
	// phase 2: forward and strafe
	btTransform start;
	btTransform end;

	m_targetPosition = m_currentPosition + walkMove;

	start.setIdentity();
	end.setIdentity();
	
	btScalar fraction = btScalar( 1.0 );
	btScalar distance2 = ( m_currentPosition - m_targetPosition ).length2();
//	printf("distance2=%f\n",distance2);

#if 0
	if( m_touchingContact )
	{
		if( m_normalizedDirection.dot( m_touchingNormal ) > btScalar( 0.0 ) )
		{
			//interferes with step movement
			//updateTargetPositionBasedOnCollision (m_touchingNormal);
		}
	}
#endif

	int maxIter = 10;

	while( ( fraction > btScalar( 0.01 ) ) && ( maxIter-- > 0 ) )
	{
		start.setOrigin (m_currentPosition);
		end.setOrigin (m_targetPosition);

		btVector3 sweepDirNegative( m_currentPosition - m_targetPosition );

		KinematicCharacter::ClosestNotMeConvexResultCallback callback( m_ghostObject, sweepDirNegative, btScalar( 0.0 ) );
		callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
		callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;

		btScalar margin = m_convexShape->getMargin();
		m_convexShape->setMargin( margin + m_addedMargin );

		if( m_useGhostObjectSweepTest )
		{
			m_ghostObject->convexSweepTest( m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );
		}
		else
		{
			collisionWorld->convexSweepTest( m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );
		}
		
		m_convexShape->setMargin( margin );
		
		fraction -= callback.m_closestHitFraction;

		if( callback.hasHit() )
		{	
			// we moved only a fraction
			btScalar hitDistance;
			hitDistance = ( callback.m_hitPointWorld - m_currentPosition).length();

//			m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);

			updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
			btVector3 currentDir = m_targetPosition - m_currentPosition;
			distance2 = currentDir.length2();

			if( distance2 > SIMD_EPSILON )
			{
				currentDir.normalize();

				/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
				if( currentDir.dot( m_normalizedDirection ) <= btScalar( 0.0 ) )
				{
					break;
				}
			}
			else
			{
//				printf("currentDir: don't normalize a zero vector\n");
				break;
			}

		}
		else
		{
			// we moved whole way
			m_currentPosition = m_targetPosition;
		}

	//	if (callback.m_closestHitFraction == 0.f)
	//		break;

	}
}

void KinematicCharacter::Controller::stepDown( btCollisionWorld* collisionWorld, btScalar dt )
{
	btTransform start;
	btTransform end;
	btTransform end_double;
	bool runonce = false;

	// phase 3: down
	/*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0;
	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep);
	btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt;
	btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; 
	m_targetPosition -= (step_drop + gravity_drop);*/

	btVector3 orig_position = m_targetPosition;
	
	btScalar downVelocity = ( ( m_verticalVelocity < btScalar( 0.0 ) )? -m_verticalVelocity : btScalar( 0.0 ) ) * dt;

	if( ( downVelocity > 0.0 ) && ( downVelocity > m_fallSpeed ) && ( m_wasOnGround || !m_wasJumping ) )
	{
		downVelocity = m_fallSpeed;
	}

	btVector3 step_drop = getUpAxisDirections()[m_upAxis] * ( m_currentStepOffset + downVelocity );
	m_targetPosition -= step_drop;

	KinematicCharacter::ClosestNotMeConvexResultCallback callback( m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine );
	callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
	callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;

    KinematicCharacter::ClosestNotMeConvexResultCallback callback2( m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine );
	callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
	callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;

	for( ;; )
	{
		start.setIdentity();
		end.setIdentity();

		end_double.setIdentity();

		start.setOrigin( m_currentPosition );
		end.setOrigin( m_targetPosition );

		//set double test for 2x the step drop, to check for a large drop vs small drop
		end_double.setOrigin( m_targetPosition - step_drop );

		if( m_useGhostObjectSweepTest )
		{
			m_ghostObject->convexSweepTest( m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );

			if( !callback.hasHit() )
			{
				//test a double fall height, to see if the character should interpolate it's fall (full) or not (partial)
				m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
			}
		}
		else
		{
			collisionWorld->convexSweepTest( m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration );

			if( !callback.hasHit() )
			{
				//test a double fall height, to see if the character should interpolate it's fall (large) or not (small)
				collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
			}
		}
	
		btScalar downVelocity2 = ( ( m_verticalVelocity < 0.0f )? -m_verticalVelocity : 0.0f ) * dt;
		bool has_hit = false;

		if( bounce_fix == true )
		{
			has_hit = callback.hasHit() || callback2.hasHit();
		}
		else
		{
			has_hit = callback2.hasHit();
		}

		if( ( downVelocity2 > 0.0 ) &&
			( downVelocity2 < m_stepHeight ) &&
			( has_hit == true ) &&
			( runonce == false ) &&
			( m_wasOnGround || !m_wasJumping ) )
		{
			//redo the velocity calculation when falling a small amount, for fast stairs motion
			//for larger falls, use the smoother/slower interpolated movement by not touching the target position

			m_targetPosition = orig_position;
			downVelocity = m_stepHeight;

			btVector3 step_drop = getUpAxisDirections()[m_upAxis] * ( m_currentStepOffset + downVelocity );

			m_targetPosition -= step_drop;
			runonce = true;

			continue; //re-run previous tests
		}

		break;
	}

	if( callback.hasHit() || ( runonce == true ) )
	{
		// we dropped a fraction of the height -> hit floor

//		btScalar fraction = ( m_currentPosition.getY() - callback.m_hitPointWorld.getY() ) / 2;
		btScalar fraction = ( m_currentPosition.getY() - callback.m_hitPointWorld.getY() ) * 0.5f;

		//printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY());

		if( bounce_fix == true )
		{
			if( full_drop == true )
			{
				m_currentPosition.setInterpolate3( m_currentPosition, m_targetPosition, callback.m_closestHitFraction );
			}
			else
			{
				//due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually
				m_currentPosition.setInterpolate3( m_currentPosition, m_targetPosition, fraction );
			}
		}
		else
		{
			m_currentPosition.setInterpolate3( m_currentPosition, m_targetPosition, callback.m_closestHitFraction );
		}

		full_drop = false;

		m_verticalVelocity = btScalar( 0.0 );
		m_verticalOffset = btScalar( 0.0 );
		m_wasJumping = false;

	}
	else
	{
		// we dropped the full height
		
		full_drop = true;

		if( bounce_fix == true )
		{
			downVelocity = ( ( m_verticalVelocity < 0.0f )? -m_verticalVelocity : 0.0f ) * dt;

			if( ( downVelocity > m_fallSpeed ) && ( m_wasOnGround || !m_wasJumping ) )
			{
				m_targetPosition += step_drop; //undo previous target change
				downVelocity = m_fallSpeed;
				step_drop = getUpAxisDirections()[m_upAxis] * ( m_currentStepOffset + downVelocity );
				m_targetPosition -= step_drop;
			}
		}

		//printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY());

		m_currentPosition = m_targetPosition;
	}
}

btVector3* KinematicCharacter::Controller::getUpAxisDirections( void )
{
	static btVector3 sUpAxisDirection[3] =
	{
		btVector3( 1.0f, 0.0f, 0.0f ),
		btVector3( 0.0f, 1.0f, 0.0f ),
		btVector3( 0.0f, 0.0f, 1.0f ),
	};
	
	return sUpAxisDirection;
}

btVector3 KinematicCharacter::Controller::getNormalizedVector( const btVector3& v )
{
	btVector3 n = v.normalized();

	if( n.length() < SIMD_EPSILON )
	{
		n.setValue( 0.0f, 0.0f, 0.0f );
	}

	return n;
}

/*
	btCharacterControllerInterface
*/

void KinematicCharacter::Controller::setWalkDirection( const btVector3& walkDirection )
{
	m_useWalkDirection = true;
	m_walkDirection = walkDirection;
	m_normalizedDirection = getNormalizedVector( m_walkDirection );
}

void KinematicCharacter::Controller::setVelocityForTimeInterval( const btVector3& velocity, btScalar timeInterval )
{
	m_useWalkDirection = false;
	m_walkDirection = velocity;
	m_normalizedDirection = getNormalizedVector( m_walkDirection );
	m_velocityTimeInterval += timeInterval;
}

void KinematicCharacter::Controller::reset( btCollisionWorld* collisionWorld )
{
	m_verticalVelocity = btScalar( 0.0 );
	m_verticalOffset = btScalar( 0.0 );
	m_wasOnGround = false;
	m_wasJumping = false;
	m_walkDirection.setValue( btScalar( 0.0 ), btScalar( 0.0 ),btScalar( 0.0 ) );
	m_velocityTimeInterval = btScalar( 0.0 );

	m_deactiveTimeCounter = btScalar( 0.0 );
	m_active = true;

	//clear pair cache
	btHashedOverlappingPairCache* cache = m_ghostObject->getOverlappingPairCache();

	while( cache->getOverlappingPairArray().size() > 0 )
	{
		cache->removeOverlappingPair( cache->getOverlappingPairArray()[0].m_pProxy0,
			cache->getOverlappingPairArray()[0].m_pProxy1,
			collisionWorld->getDispatcher() );
	}
}

void KinematicCharacter::Controller::warp( const btVector3& origin )
{
	btTransform xform;

	xform.setIdentity();
	xform.setOrigin( origin );
	m_ghostObject->setWorldTransform( xform );

	m_deactiveTimeCounter = btScalar( 0.0 );
	m_active = true;
}

void KinematicCharacter::Controller::preStep( btCollisionWorld* collisionWorld )
{
	int numPenetrationLoops = 0;

	m_touchingContact = false;

	while( recoverFromPenetration( collisionWorld ) )
	{
		numPenetrationLoops++;

		m_touchingContact = true;

		if( numPenetrationLoops > 4 )
		{
			//printf("character could not recover from penetration = %d\n", numPenetrationLoops);
			break;
		}
	}

	m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
	m_targetPosition = m_currentPosition;
}

void KinematicCharacter::Controller::playerStep( btCollisionWorld* collisionWorld, btScalar dt )
{
	// quick check...
	if( !m_useWalkDirection && m_velocityTimeInterval <= 0.0 )
	{
		// no motion
		m_deactiveTimeCounter = m_deactivationElapsedTime + SIMD_EPSILON;
		m_active = false;
		return;
	}

	m_wasOnGround = onGround();

	// Update fall velocity.
	m_verticalVelocity -= m_gravity * dt;
	if( m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed )
	{
		m_verticalVelocity = m_jumpSpeed;
	}

	if( m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs( m_fallSpeed ) )
	{
		m_verticalVelocity = -btFabs( m_fallSpeed );
	}

	m_verticalOffset = m_verticalVelocity * dt;

	btTransform xform;
	xform = m_ghostObject->getWorldTransform ();

	stepUp( collisionWorld );

	if( m_useWalkDirection )
	{
		stepForwardAndStrafe( collisionWorld, m_walkDirection );
	}
	else
	{
		// still have some time left for moving!
		btScalar dtMoving = ( dt < m_velocityTimeInterval ) ? dt : m_velocityTimeInterval;

		m_velocityTimeInterval -= dt;

		// how far will we move while we are moving?
		btVector3 move = m_walkDirection * dtMoving;

		// okay, step
		stepForwardAndStrafe( collisionWorld, move );
	}

	stepDown( collisionWorld, dt );

	// active check - additional
	if( ( m_currentPosition - xform.getOrigin() ).length2() < m_activeThreshold2 )
	{
		m_deactiveTimeCounter += dt;
	}
	else
	{
		m_deactiveTimeCounter = btScalar( 0.0 );
	}

	if( m_deactiveTimeCounter >= m_deactivationElapsedTime )
	{
		m_deactiveTimeCounter = m_deactivationElapsedTime + SIMD_EPSILON;
		m_active = !onGround();
	}
	else
	{
		m_active = true;
	}

#if 0
	if( m_active == true )
	{
		MIX_LOG_INFO_NO( L"KC : Active" );
	}
	else
	{
		MIX_LOG_INFO_NO( L"KC : Dective" );
	}
#endif

	xform.setOrigin( m_currentPosition );
	m_ghostObject->setWorldTransform (xform);
}

bool KinematicCharacter::Controller::canJump( void ) const
{
	return onGround();
}

void KinematicCharacter::Controller::jump( void )
{
	if( !canJump() )
	{
		return;
	}

	m_verticalVelocity = m_jumpSpeed;
	m_wasJumping = true;

#if 0
	currently no jumping.
	btTransform xform;
	m_rigidBody->getMotionState()->getWorldTransform (xform);
	btVector3 up = xform.getBasis()[1];
	up.normalize ();
	btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
	m_rigidBody->applyCentralImpulse (up * magnitude);
#endif
}

bool KinematicCharacter::Controller::onGround( void ) const
{
	return ( ( m_verticalVelocity == 0.0f ) && ( m_verticalOffset == 0.0f ) );
}

void KinematicCharacter::Controller::setUpInterpolate( bool value )
{
	m_interpolateUp = value;
}

/*
	btActionInterface
*/

void KinematicCharacter::Controller::updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime )
{
	preStep( collisionWorld );
	playerStep( collisionWorld, deltaTime );
}

void KinematicCharacter::Controller::debugDraw( btIDebugDraw* debugDrawer )
{
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// KinematicCharacter
////////////////////////////////////////////////////////////////////////////////////////////////////

KinematicCharacter* KinematicCharacter::CreateInstance( void )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, KinematicCharacter );
}

KinematicCharacter::KinematicCharacter( void ) :
Mix::Dynamics::Object( Mix::Dynamics::OF_CHARACTER, Mix::Dynamics::OF_CHARACTER_MASK, Mix::Dynamics::DD_AXIS | Mix::Dynamics::DD_WIREFRAME ),
m_pShape( NULL ),
m_pObject( NULL ),
m_pController( NULL ),
m_pContext( NULL ),
m_Height( 0.0f ),
m_Radius( 0.0f ),
m_MaxFallSpeed( 55.0f ),
m_InitalJumpSpeed( 10.0f ),
m_UserIndex( 0 ),
m_pUserPtr( NULL )
{
}

KinematicCharacter::~KinematicCharacter( void )
{
	MIX_LIB_DELETE( m_pController );
	MIX_LIB_DELETE( m_pObject );
	MIX_LIB_DELETE( m_pShape );

	MIX_LIB_DELETE_T( ObjectContext, m_pContext );
}

Boolean KinematicCharacter::Initialize( Float32 height, Float32 radius, Float32 stepHeight, const wchar_t* pDebugName )
{
	MIX_ASSERT( m_pShape == NULL );
	MIX_ASSERT( m_pObject == NULL );
	MIX_ASSERT( m_pController == NULL );
	MIX_ASSERT( m_pContext == NULL );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// パラメータの設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_Height = height;
	m_Radius = radius;

	m_StepHeight = stepHeight;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コンテキストの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pContext = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, Mix::Dynamics::ObjectContext, this, pDebugName );
	if( m_pContext == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : DebugName[%s]", KinematicCharacter::FAILED_CREATE, Mix::STR_OUTOFMEMORY, pDebugName );
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// カプセルシェイプの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pShape = MIX_LIB_NEW btCapsuleShape( m_Radius, max( 0.0f, m_Height - ( m_Radius * 2.0f ) ) );
	if( m_pShape == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : DebugName[%s]", KinematicCharacter::FAILED_CREATE, Mix::STR_OUTOFMEMORY, pDebugName );
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リフレッシュしてゴーストオブジェクトとキャラクターコントローラーを作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

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

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// デバッグ
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Debug_SetDrawAxisScaling( m_Radius );

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

	return True;
}

Boolean KinematicCharacter::Refresh( void )
{
	MIX_ASSERT( m_pContext != NULL );

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

	Object::Bullet_BeginRefresh();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 解放
	////////////////////////////////////////////////////////////////////////////////////////////////////

	MIX_LIB_DELETE( m_pController );
	MIX_LIB_DELETE( m_pObject );

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

	m_pObject = MIX_LIB_NEW btPairCachingGhostObject();
	if( m_pObject != NULL )
	{
		const Mix::Dynamics::MATERIAL& mtrl = m_pContext->GetMaterial();

		m_pObject->setCollisionShape( m_pShape );
		m_pObject->setCollisionFlags( btCollisionObject::CF_CHARACTER_OBJECT );
		m_pObject->setFriction( mtrl.friction );
		m_pObject->setRestitution( mtrl.restitution );
		m_pObject->setUserPointer( m_pContext );
	}
	else
	{
		return False;
	}

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

	m_pController = MIX_LIB_NEW KinematicCharacter::Controller( m_pObject, m_pShape, m_StepHeight );
	if( m_pController != NULL )
	{
		m_pController->setUseGhostSweepTest( true );
		m_pController->setUpInterpolate( true );
		m_pController->setFallSpeed( m_MaxFallSpeed );
		m_pController->setJumpSpeed( m_InitalJumpSpeed );
		m_pController->setWalkDirection( btVector3( 0.0f, -KinematicCharacter::MIN_LINEAR_VELOCITY, 0.0f ) ); //これやっとかないと落ちる
	}
	else
	{
		return False;
	}

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

	Object::Bullet_EndRefresh();

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

	return True;
}

Mix::Dynamics::ObjectContext* KinematicCharacter::GetContextPtr( void ) const
{
	return m_pContext;
}

btCollisionObject* KinematicCharacter::Bullet_GetCollisionObjectPtr( void ) const
{
	return m_pObject;
}

btActionInterface* KinematicCharacter::Bullet_GetActionInterfacePtr( void ) const
{
	return m_pController;
}

btCharacterControllerInterface* KinematicCharacter::Bullet_GetCharacterControllerInterfacePtr( void ) const
{
	return m_pController;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Mix::Dynamics::IKinematicCharacter
////////////////////////////////////////////////////////////////////////////////////////////////////

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

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

void KinematicCharacter::SetGravity( Float32 gravity )
{
	m_pController->setGravity( gravity );
}

Float32 KinematicCharacter::GetGravity( void ) const
{
	return m_pController->getGravity();
}

void KinematicCharacter::SetMaxFallSpeed( Float32 speed )
{
	m_MaxFallSpeed = max( 0.0f, speed );
	m_pController->setFallSpeed( m_MaxFallSpeed );
}

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

void KinematicCharacter::SetInitalJumpSpeed( Float32 speed )
{
	m_InitalJumpSpeed = max( 0.0f, speed );
	m_pController->setJumpSpeed( m_InitalJumpSpeed );
}

Float32 KinematicCharacter::GetInitalJumpSpeed( void ) const
{
	return m_InitalJumpSpeed;
}

void KinematicCharacter::SetSlopeLimit( Float32 rad )
{
	m_pController->setMaxSlope( rad );
}

Float32 KinematicCharacter::GetStepHeight( void ) const
{
	return m_StepHeight;
}

void KinematicCharacter::SetStepHeight( Float32 height )
{
	m_StepHeight = max( 0.0f, height );

	Refresh();
}

Float32 KinematicCharacter::GetSlopeLimit( void ) const
{
	return m_pController->getMaxSlope();
}

Float32 KinematicCharacter::GetActiveThreshold( void ) const
{
	return m_pController->getActiveThreshold();
}

void KinematicCharacter::SetActiveThreshold( Float32 threshold )
{
	m_pController->setActiveThreshold( threshold );
}

Float32 KinematicCharacter::GetDeactivationElapsedTime( void ) const
{
	return m_pController->getDeactivationElapsedTime();
}

void KinematicCharacter::SetDeactivationElapsedTime( Float32 elapsedTime )
{
	m_pController->setDeactivationElapsedTime( elapsedTime );
}

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

void KinematicCharacter::SetLinearVelocity( const Mix::Vector3& velocity )
{
	m_LinearVelocity = velocity;

	if( KinematicCharacter::MIN_LINEAR_VELOCITY < m_LinearVelocity.GetLength() )
	{
		m_pController->setWalkDirection( ToBulletVector3( m_LinearVelocity ) );
	}
	else
	{
		m_pController->setWalkDirection( btVector3( 0.0f, -KinematicCharacter::MIN_LINEAR_VELOCITY, 0.0f ) );
	}
}

Boolean KinematicCharacter::OnGround( void ) const
{
	return ( m_pController->onGround() == true );
}

Boolean KinematicCharacter::IsActive( void ) const
{
	return m_pController->isActive();
}

void KinematicCharacter::Jump( void )
{
	m_pController->jump();
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Mix::Dynamics::IObject
////////////////////////////////////////////////////////////////////////////////////////////////////

Mix::Dynamics::IObject::TYPE KinematicCharacter::GetType( void ) const
{
	return Mix::Dynamics::IObject::KINEMATIC_CHARACTER;
}

Boolean KinematicCharacter::GetShape( Mix::Dynamics::IShape** ppShape )
{
	return False;
}

Float32 KinematicCharacter::GetShapeMargin( void ) const
{
	return m_pShape->getMargin();
}

void KinematicCharacter::SetShapeMargin( Float32 margin )
{
	m_pShape->setMargin( margin );
}

const Mix::Dynamics::MATERIAL& KinematicCharacter::GetMaterial( void ) const
{
	return m_pContext->GetMaterial();
}

void KinematicCharacter::SetMaterial( const Mix::Dynamics::MATERIAL& material )
{
	m_pContext->SetMaterial( material );

	m_pObject->setFriction( material.friction );
	m_pObject->setRestitution( material.restitution );
}

UInt16 KinematicCharacter::GetFilterGroup( void ) const
{
	return Object::Bullet_GetFilterGroup();
}

void KinematicCharacter::SetFilterGroup( UInt16 filterGroup )
{
	Object::Bullet_SetFilterGroup( filterGroup );
}

UInt16 KinematicCharacter::GetFilterMask( void ) const
{
	return Object::Bullet_GetFilterMask();
}

void KinematicCharacter::SetFilterMask( UInt16 filterMask )
{
	Object::Bullet_SetFilterMask( filterMask );
}

void KinematicCharacter::SetWorldTransform( const Mix::Quaternion& rot, const Mix::Vector3& pos )
{
#if 1

	btTransform tr;

	tr.setRotation( Mix::Dynamics::ToBulletQuaternion( rot ) );
	tr.setOrigin( Mix::Dynamics::ToBulletVector3( pos ) );

	m_pObject->setWorldTransform( tr );

#else

	m_pController->warp( Mix::Dynamics::ToBulletVector3( pos ) ); //ワールドトランスフォームを初期化されてしまうので、先に位置を設定しておく
	m_pObject->getWorldTransform().setRotation( Mix::Dynamics::ToBulletQuaternion( rot ) );

#endif
}

Mix::Quaternion KinematicCharacter::GetWorldRotation( void ) const
{
	return Mix::Dynamics::ToMixQuaternion( m_pObject->getWorldTransform().getRotation() );
}

void KinematicCharacter::SetWorldRotation( const Mix::Quaternion& rot )
{
	m_pObject->getWorldTransform().setRotation( Mix::Dynamics::ToBulletQuaternion( rot ) );
}

Mix::Vector3 KinematicCharacter::GetWorldPosition( void ) const
{
	return Mix::Dynamics::ToMixVector3( m_pObject->getWorldTransform().getOrigin() );
}

void KinematicCharacter::SetWorldPosition( const Mix::Vector3& pos )
{
	m_pObject->getWorldTransform().setOrigin( Mix::Dynamics::ToBulletVector3( pos ) );
}

Mix::Matrix4x4 KinematicCharacter::GetWorldMatrix( void ) const
{
	return Mix::Dynamics::ToMixMatrix4x4( m_pObject->getWorldTransform() );
}

Boolean KinematicCharacter::IsInWorld( void ) const
{
	return Object::IsInWorld();
}

Mix::Geometry::AABB KinematicCharacter::GetBounds( void ) const
{
	Float32 hh = m_pShape->getRadius();
	Float32 hv = hh + m_pShape->getHalfHeight();

	return Mix::Geometry::AABB( Mix::Vector3( -hh, -hv, -hh ), Mix::Vector3( +hh, +hv, +hh ) );
}

Boolean KinematicCharacter::AddListener( Mix::Dynamics::IObjectListener* pListener )
{
	return m_pContext->AddListener( pListener );
}

void KinematicCharacter::RemoveListener( Mix::Dynamics::IObjectListener* pListener )
{
	m_pContext->RemoveListener( pListener );
}

Int32 KinematicCharacter::GetUserIndex( void ) const
{
	return m_UserIndex;
}

void KinematicCharacter::SetUserIndex( Int32 index )
{
	m_UserIndex = index;
}

void* KinematicCharacter::GetUserPtr( void ) const
{
	return m_pUserPtr;
}

void KinematicCharacter::SetUserPtr( void* pData )
{
	m_pUserPtr = pData;
}

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

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

Float32 KinematicCharacter::Debug_GetDrawAxisScaling( void ) const
{
	return m_DebugDrawAxisScaling;
}

void KinematicCharacter::Debug_SetDrawAxisScaling( Float32 scaling )
{
	m_DebugDrawAxisScaling = max( 0.0f, scaling );
}

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

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

		if( MIX_TESTBIT( m_DebugDrawFlags, Mix::Dynamics::DD_AXIS ) == Mix::Dynamics::DD_AXIS )
		{
			pPerspectiveRenderer->SetColor( Mix::Vector4( 1.0f, 1.0f, 1.0f, opacity ) );
			pPerspectiveRenderer->SetMatrix( GetWorldMatrix() );

			pPerspectiveRenderer->AddAxis( Debug_GetDrawAxisScaling() );
		}

		////////////////////////////////////////////////////////////////////////////////////////////////////
		// ワイヤーフレーム
		////////////////////////////////////////////////////////////////////////////////////////////////////

		if( MIX_TESTBIT( m_DebugDrawFlags, Mix::Dynamics::DD_WIREFRAME ) == Mix::Dynamics::DD_WIREFRAME )
		{
			pPerspectiveRenderer->SetColor( Mix::Dynamics::Debug::GetColor( Mix::Dynamics::DDC_KINEMATIC_CHARACTER, opacity ) );
			pPerspectiveRenderer->SetMatrix( Mix::Matrix4x4( Mix::Quaternion::Identity(), GetWorldPosition() ) );
//			pPerspectiveRenderer->SetMatrix( GetWorldMatrix() );

			pPerspectiveRenderer->AddCapsule( 1, m_pShape->getHalfHeight() * 2.0f + m_pShape->getRadius() * 2.0f, m_pShape->getRadius() );
		}

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

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

}}
