#include "physic_bullet/bullet_character_controller.h"
		#include "LinearMath/btIDebugDraw.h"
		#include "nstring/nstring.h"

		using namespace GS;


//------------------------------------------------------------------------------
void		btCustomCharacterController::debugDraw(btIDebugDraw *idebug)
{
	String output;

	output << "Vertical Velocity: " << mVerticalVelocity << "\n";
	output << "OnGround: " << (mGroundContact ? "Yes" : "No") << "\n";
	output << "Ground.y: " << mGroundNormal.y() << "\n";
	output << "Step high: " << dbg_step_high << "\n";
	output << "Down sweep: " << dbg_down_sweep_hit << "\n";

	idebug->draw3dText(mCurrentPosition, output.c_str());
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
btVector3	btCustomCharacterController::computeReflectionDirection(const btVector3 & direction, const btVector3 & normal)
{	return direction - (btScalar(2) * direction.dot(normal)) * normal;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
btCustomCharacterController::btCustomCharacterController(btPairCachingGhostObject * ghostObject, btConvexShape * convexShape, btScalar stepHeight, btCollisionWorld * collisionWorld, int upAxis)
{
	mUpAxis = upAxis;
	mAddedMargin = 0.02;
	mWalkDirection.setValue(0, 0, 0);
//	mUseGhostObjectSweepTest = true;
	mGhostObject = ghostObject;
	mStepHeight = stepHeight;
	mTurnAngle = 0;
	mConvexShape = mStandingConvexShape = convexShape;
	mUseWalkDirection = true;
	mVelocityTimeInterval = 0;
	mVerticalOffset = 0;
	mVerticalVelocity = 0;
	mGravity = 9.8 * 3.0;
	mFallSpeed = 9.8;
	mJumpSpeed = 10;
//	mWasOnGround = false;
//	mWasJumping = false;
	setMaxSlope(btRadians(45));
	mCollisionWorld = collisionWorld;
//	mCanStand = true;
	mCurrentPosition.setValue(0, 0, 0);
	mMass = 20;
	mGroundContact = false;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void btCustomCharacterController::setDuckingConvexShape(btConvexShape * shape)
{	mDuckingConvexShape = shape;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setRBForceImpulseBasedOnCollision()
{
	if	(mWalkDirection.isZero())
		return;

	for	(int i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); ++i)
	{
		btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];

		btRigidBody *rb = (btRigidBody*)collisionPair->m_pProxy1->m_clientObject;

		if	(mMass > rb->getInvMass())
		{
			btScalar resultMass = mMass - rb->getInvMass();
			btVector3 reflection = computeReflectionDirection(mWalkDirection * resultMass, getNormalizedVector(mWalkDirection));
			rb->applyCentralImpulse(reflection * -1);
		}
	}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setVelocityForTimeInterval(const btVector3 & velocity, btScalar timeInterval)
{
	mUseWalkDirection = false;
	mWalkDirection = velocity;
	mNormalizedDirection = getNormalizedVector(mWalkDirection);
	mVelocityTimeInterval = timeInterval;
}

void		btCustomCharacterController::warp(const btVector3 & origin)
{
	btTransform xform;
	xform.setIdentity();
	xform.setOrigin(origin);
	mGhostObject->setWorldTransform(xform);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool		btCustomCharacterController::SweepTest(const btVector3 &src, const btVector3 &dst, btScalar &hitFraction, btVector3 *normal, btVector3 *hit)
{
	btTransform start, end;
	start.setIdentity(); end.setIdentity();
	start.setOrigin(src); end.setOrigin(dst);

	ClosestNotMeConvexResultCallback callback(mGhostObject, getUpAxisDirection(), 0);
	callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
	callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask;

	mGhostObject->convexSweepTest(mConvexShape, start, end, callback, mCollisionWorld->getDispatchInfo().m_allowedCcdPenetration);

	hitFraction = callback.hasHit() ? callback.m_closestHitFraction : btScalar(1);
	if	(normal)
		*normal = callback.m_hitNormalWorld;
	if	(hit)
		*hit = callback.m_hitPointWorld;

	return callback.hasHit();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool		btCustomCharacterController::recoverFromPenetration(const btVector3 &step_direction)
{
return false;
	mCollisionWorld->getDispatcher()->dispatchAllCollisionPairs(mGhostObject->getOverlappingPairCache(), mCollisionWorld->getDispatchInfo(), mCollisionWorld->getDispatcher());

	bool penetration = false;
	for (int i = 0; i < mGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); ++i)
	{
		btBroadphasePair *collisionPair = &mGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];

		mManifoldArray.resize(0);
		if	(collisionPair->m_algorithm)
			collisionPair->m_algorithm->getAllContactManifolds(mManifoldArray);

		for (int j = 0; j < mManifoldArray.size(); ++j)
		{
			btPersistentManifold *manifold = mManifoldArray[j];
			btScalar directionSign = manifold->getBody0() == mGhostObject ? btScalar(1) : btScalar(-1);

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

				btScalar dist = pt.getDistance();
				btVector3 normal = pt.m_normalWorldOnB * directionSign;

				if	(dist < 0.0)
				{
					penetration = true;

					if	(normal.y() > 0.9)
						normal = btVector3(0, 1, 0);	// prevent sliding down slopes

					mCurrentPosition -= normal * dist * btScalar(0.1);
				}
			}
		}
	}
	return penetration;
}
//------------------------------------------------------------------------------


#include "log/log.h"

enum
{
	UpSweep,
	ForwardSweep,
	DownSweep
};

//------------------------------------------------------------------------------
bool		btCustomCharacterController::SweepAndSlide(btVector3 &from, btVector3 &to, int sweep)
{
	bool hit = false;
	for (int it = 0; it < 4; ++it)
	{
		btScalar hit_fraction;
		btVector3 hit_normal;
		if	(!SweepTest(from, to, hit_fraction, &hit_normal))
			return hit;

		switch (sweep)
		{
			case ForwardSweep:
				if	(hit_normal.y() < 0.75)
				{
					hit_normal.setY(0.0);
					hit_normal.normalize();
				}
				else
					hit = true;
				break;
		}

		from += (to - from) * hit_fraction;
		to -= hit_normal * (to - from).dot(hit_normal);

		if	((to - from).length2() < btScalar(0.0001))
			break;
	}
	return hit;
}
//------------------------------------------------------------------------------

void		btCustomCharacterController::performStep(btScalar dt)
{
	btScalar hit_fraction;
	btVector3 hit_normal, hit_point;

mStepHeight = 0.25;

	// up sweep
	btVector3 step_height = getUpAxisDirection() * mStepHeight;

	btVector3 wpos = mGhostObject->getWorldTransform().getOrigin();
	btVector3 tpos = wpos + step_height;

	SweepTest(wpos, tpos, hit_fraction, &hit_normal);
	tpos = wpos + (tpos - wpos) * hit_fraction;

	// forward sweep
	bool forward_hit = false;

	if	(mWalkDirection.length2() > 0.0)
	{
		wpos = tpos;
		tpos += mWalkDirection;

		forward_hit = SweepAndSlide(wpos, tpos, ForwardSweep);
	}

	// down sweep
	btVector3 g = btVector3(0, -9, 0) * dt;

	wpos = tpos;
	tpos -= step_height;
	tpos += g;

	bool down_hit = SweepTest(wpos, tpos, hit_fraction, &hit_normal);
	tpos = wpos + (tpos - wpos) * hit_fraction;

	// landing on a steep slope higher than we starter, revert height change.
	if	(down_hit && (hit_normal.y() < 0.75))
	{
		tpos += hit_normal * 0.2;
		wpos = tpos;
		tpos = wpos - btVector3(0, 4, 0);

		SweepTest(wpos, tpos, hit_fraction);
		tpos = wpos + (tpos - wpos) * hit_fraction;
	}



	mCurrentPosition = tpos;
	return;
//	}
/*
	// perform high sweep to step above small obstacle
	btVector3 step_height = getUpAxisDirection() * mStepHeight;

	wpos = mGhostObject->getWorldTransform().getOrigin() + step_height;
	tpos = wpos + mWalkDirection;

	if	(!SweepAndSlide(wpos, tpos, HighSweep))
		return;		// high sweep is not cutting it either... drop its result entirely

	// step down from the high sweep
	wpos = tpos;
	tpos -= step_height;

	if	(!SweepAndSlide(wpos, tpos, DownSweep))
	{
		mVerticalVelocity = 0;
		return;
	}


//	else
	{
		mVerticalVelocity = btClamped(mVerticalVelocity - mGravity * dt, -mFallSpeed, mJumpSpeed);

		wpos = tpos;
		tpos += btVector3(0, mVerticalVelocity, 0) * dt;

		SweepAndSlide(wpos, tpos, GravitySweep);
	}

	// commit
	mCurrentPosition = tpos;
*/
}


//------------------------------------------------------------------------------
void		btCustomCharacterController::preStep(btCollisionWorld *collisionWorld)
{}
void		btCustomCharacterController::playerStep(btCollisionWorld * collisionWorld, btScalar dt)
{
	if	(!mUseWalkDirection && mVelocityTimeInterval <= 0)
		return;

performStep(dt);
#if		0
	mCurrentPosition = mGhostObject->getWorldTransform().getOrigin();

	// Apply gravity.
	mVerticalVelocity = btClamped(mVerticalVelocity - mGravity * dt, -mFallSpeed, mJumpSpeed);

	// Compute total step velocity.
	mTouchingContact = false;
	for (int n = 0; (n < 4) && recoverFromPenetration(mWalkDirection + btVector3(0, -1, 0) * mVerticalVelocity); ++n)
		mTouchingContact = true;

	// Perform sweep tests.
	btVector3 stepHeight = getUpAxisDirection() * mStepHeight;
	btVector3 stepHigh = mCurrentPosition + stepHeight;

	btScalar hitFraction;

	mGroundContact = false;
	if	(SweepTest(stepHigh, stepHigh + mWalkDirection, hitFraction, &mGroundNormal))
	{
		btVector3 hit = stepHigh + mWalkDirection * hitFraction;
		if	(mGroundNormal.y() > 0.6)	// on ground, take step
			mCurrentPosition = hit;
	}
	else
	{
		stepHigh += mWalkDirection;	// take the whole walk step

		btVector3 g = stepHeight - mVerticalVelocity * getUpAxisDirection();

		bool down_sweep_hit = SweepTest(stepHigh, stepHigh - g, hitFraction, &mGroundNormal);

		if	(!down_sweep_hit)
		{
			mCurrentPosition += mVerticalVelocity * getUpAxisDirection();	// free-falling
		}
		else
		{
			mGroundContact = mGroundNormal.y() > 0.6;

			btVector3 dp = stepHigh - g * hitFraction;

			if	(dp.y() > mCurrentPosition.y())	// going up a slope
			{
				if	(mGroundContact)
					mCurrentPosition = dp;	// ok if on ground
			}
			else
				mCurrentPosition = dp;
		}
	}
#endif



	//
	if	(mGroundContact)
		mVerticalVelocity = 0.0;

	//
	btTransform xform = mGhostObject->getWorldTransform();
	xform.setOrigin(mCurrentPosition);
	mGhostObject->setWorldTransform(xform);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setFallSpeed(btScalar fallSpeed)
{	mFallSpeed = fallSpeed;	}
void		btCustomCharacterController::setJumpSpeed(btScalar jumpSpeed)
{	mJumpSpeed = jumpSpeed;	}
void btCustomCharacterController::setMaxJumpHeight(btScalar maxJumpHeight)
{	mMaxJumpHeight = maxJumpHeight;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool		btCustomCharacterController::canJump() const
{	return onGround();	}
void		btCustomCharacterController::jump()
{
	if	(!canJump())
		return;

	mVerticalVelocity = mJumpSpeed;
//	mWasJumping = true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::duck()
{
	mConvexShape = mDuckingConvexShape;
	mGhostObject->setCollisionShape(mDuckingConvexShape);

	btTransform xform;
	xform.setIdentity();
	xform.setOrigin(mCurrentPosition + btVector3(0, 0.1, 0));
	mGhostObject->setWorldTransform(xform);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::stand()
{
	mConvexShape = mStandingConvexShape;
	mGhostObject->setCollisionShape(mStandingConvexShape);
}
bool		btCustomCharacterController::canStand()
{
	return true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setGravity(const btScalar gravity)
{	mGravity = gravity;	}
btScalar	btCustomCharacterController::getGravity() const
{	return mGravity;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setMaxSlope(btScalar slopeRadians)
{
	mMaxSlopeRadians = slopeRadians;
	mMaxSlopeCosine = btCos(slopeRadians);
}
btScalar	btCustomCharacterController::getMaxSlope() const
{	return mMaxSlopeRadians;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
bool		btCustomCharacterController::onGround() const
{	return mGroundContact;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setWalkDirection(const btVector3 & walkDirection)
{
	mUseWalkDirection = true;
	mWalkDirection = walkDirection;
	mNormalizedDirection = getNormalizedVector(mWalkDirection);
}
void		btCustomCharacterController::setWalkDirection(const btScalar x, const btScalar y, const btScalar z)
{
	mUseWalkDirection = true;
	mWalkDirection.setValue(x, y, z);
	mNormalizedDirection = getNormalizedVector(mWalkDirection);
}
btVector3	btCustomCharacterController::getWalkDirection() const
{	return mWalkDirection;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
btVector3	btCustomCharacterController::getPosition() const
{	return mCurrentPosition;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::setOrientation(const btQuaternion &orientation)
{
	btTransform xform;
	xform = mGhostObject->getWorldTransform();
	xform.setRotation(orientation);
	mGhostObject->setWorldTransform(xform);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void		btCustomCharacterController::updateAction(btCollisionWorld *collisionWorld, btScalar dt)
{
	preStep(collisionWorld);
	playerStep(collisionWorld, dt);
}
//------------------------------------------------------------------------------