#include <iostream>
#include <OgreBullet.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#include <btBulletCollisionCommon.h>
#include <btBulletDynamicsCommon.h>

struct objects {
	btDynamicsWorld *world;
	btRigidBody *groundBody;
	btPairCachingGhostObject *characterBody;
	btPairCachingGhostObject *waterBody;
};
void setupGround(btDynamicsWorld *world, struct objects *objects)
{
	// Static ground
	btCollisionShape *groundShape =
		new btBoxShape(btVector3(1000, 1, 1000));
	btDefaultMotionState *groundMotionState = new btDefaultMotionState(
		btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -10, 0)));
	btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(
		0, groundMotionState, groundShape, btVector3(0, 0, 0));
	btRigidBody *groundRigidBody = new btRigidBody(groundRigidBodyCI);
	world->addRigidBody(groundRigidBody, 2, 0x7fffffff & ~16);
	objects->groundBody = groundRigidBody;
}
void setupCharacter(btDynamicsWorld *world, struct objects *objects)
{
	btCompoundShape *shape = new btCompoundShape();
	btCapsuleShape *subshape = new btCapsuleShape(0.2f, 1.5f);
	btTransform shapePos;
	shapePos.setIdentity();
	shapePos.setOrigin(btVector3(0, 0.75f, 0));
	shape->addChildShape(shapePos, subshape);
	btPairCachingGhostObject *characterBody =
		new btPairCachingGhostObject();
	characterBody->setCollisionFlags(
		characterBody->getCollisionFlags() |
		btCollisionObject::CF_NO_CONTACT_RESPONSE |
		btCollisionObject::CF_KINEMATIC_OBJECT);
	btTransform bodyPos;
	bodyPos.setIdentity();
	bodyPos.setOrigin(btVector3(0, 0.4f, 0));
	characterBody->setWorldTransform(bodyPos);
	characterBody->setCollisionShape(shape);
	world->addCollisionObject(characterBody, 1, 0x7fffffff);
	objects->characterBody = characterBody;
}
void setupWater(btDynamicsWorld *world, struct objects *objects)
{
	btCompoundShape *shape = new btCompoundShape();
	btBoxShape *subshape =
		new btBoxShape(btVector3(1000.0f, 100.0f, 1000.0f));
	btTransform shapePos;
	shapePos.setIdentity();
	shapePos.setOrigin(btVector3(0, -100, 0));
	shape->addChildShape(shapePos, subshape);
	btPairCachingGhostObject *waterBody = new btPairCachingGhostObject();
	waterBody->setCollisionFlags(waterBody->getCollisionFlags() |
				     btCollisionObject::CF_NO_CONTACT_RESPONSE |
				     btCollisionObject::CF_KINEMATIC_OBJECT);
	btTransform bodyPos;
	bodyPos.setIdentity();
	waterBody->setWorldTransform(bodyPos);
	waterBody->setCollisionShape(shape);
	world->addCollisionObject(waterBody, 16, 0x7fffffff & ~2);
	objects->waterBody = waterBody;
}
struct DeepPenetrationContactResultCallback : public btManifoldResult {
	DeepPenetrationContactResultCallback(
		const btCollisionObjectWrapper *body0Wrap,
		const btCollisionObjectWrapper *body1Wrap)
		: btManifoldResult(body0Wrap, body1Wrap)
		, mPenetrationDistance(0)
		, mOtherIndex(0)
	{
	}
	float mPenetrationDistance;
	int mOtherIndex;
	btVector3 mNormal, mPoint;
	void reset()
	{
		mPenetrationDistance = 0.0f;
	}
	bool hasHit()
	{
		return mPenetrationDistance < 0.0f;
	}
	virtual void addContactPoint(const btVector3 &normalOnBInWorld,
				     const btVector3 &pointInWorldOnB,
				     btScalar depth)
	{
		std::cout
			<< "contact: " << Ogre::Bullet::convert(pointInWorldOnB)
			<< " " << Ogre::Bullet::convert(normalOnBInWorld)
			<< "\n";
		if (mPenetrationDistance > depth) { // Has penetration?

			const bool isSwapped =
				m_manifoldPtr->getBody0() !=
				m_body0Wrap->getCollisionObject();
			mPenetrationDistance = depth;
			mOtherIndex = isSwapped ? m_index0 : m_index1;
			mPoint = isSwapped ? (pointInWorldOnB +
					      (normalOnBInWorld * depth)) :
					     pointInWorldOnB;

			mNormal = isSwapped ? normalOnBInWorld * -1 :
					      normalOnBInWorld;
		}
	}
};
void dumpCompoundShape(const char *what, const btCollisionObject *body)
{
	if (body->getCollisionShape()->isCompound()) {
		const btCompoundShape *shape =
			static_cast<const btCompoundShape *>(
				body->getCollisionShape());
		int i;
		for (i = 0; i < shape->getNumChildShapes(); i++) {
			btTransform transform = body->getWorldTransform() *
						shape->getChildTransform(i);
			std::cout << what << ": " << " shape: " << i << ": "
				  << transform.getOrigin().getX() << ", ";
			std::cout << transform.getOrigin().getY() << ", ";
			std::cout << transform.getOrigin().getZ();
			std::cout << " convex: "
				  << shape->getChildShape(i)->isConvex();
			std::cout << " shape name: "
				  << shape->getChildShape(i)->getName();
			switch (shape->getChildShape(i)->getShapeType()) {
			case 0: {
				const btBoxShape *box =
					static_cast<const btBoxShape *>(
						shape->getChildShape(i));
				btVector3 hextents =
					box->getHalfExtentsWithoutMargin();
				std::cout << " box: " << hextents.getX() << ", "
					  << hextents.getY() << ", "
					  << hextents.getZ();
			} break;
			case 10: {
				const btCapsuleShape *capsule =
					static_cast<const btCapsuleShape *>(
						shape->getChildShape(i));
				float hh = capsule->getHalfHeight();
				float r = capsule->getRadius();
				std::cout << " capsule: " << hh << ", " << r;
			} break;
			default:
				std::cout << " shape type: "
					  << shape->getChildShape(i)
						     ->getShapeType();
				break;
			}
			std::cout << std::endl;
		}
	}
}
void showContacts(btCollisionWorld *world, struct objects *objects)
{
	int numManifolds = world->getDispatcher()->getNumManifolds();
	if (numManifolds == 0)
		std::cout << "No contacts in world\n";
	for (int i = 0; i < numManifolds; i++) {
		btPersistentManifold *contactManifold =
			world->getDispatcher()->getManifoldByIndexInternal(i);
		const btCollisionObject *obA = contactManifold->getBody0();
		const btCollisionObject *obB = contactManifold->getBody1();

		int numContacts = contactManifold->getNumContacts();
		for (int j = 0; j < numContacts; j++) {
			btManifoldPoint &pt =
				contactManifold->getContactPoint(j);
			if (pt.getDistance() < 0.f) {
				const btVector3 &ptA = pt.getPositionWorldOnA();
				const btVector3 &ptB = pt.getPositionWorldOnB();
				const btVector3 &normalOnB =
					pt.m_normalWorldOnB;
				std::cout << "contact: " << i << " " << j << " "
					  << ptA.getX() << ", " << ptA.getY()
					  << ", " << ptA.getZ() << std::endl;
			}
		}
	}
}
int main()
{
	struct objects objects;
	btDefaultCollisionConfiguration collisionConfig;
	btCollisionDispatcher dispatcher(&collisionConfig);
	btCollisionDispatcher *dispatch = &dispatcher;
	btDbvtBroadphase broadphase;
	btSequentialImpulseConstraintSolver solver;
	btDiscreteDynamicsWorld world(&dispatcher, &broadphase, &solver,
				      &collisionConfig);
	world.setGravity(btVector3(0, -9.81, 0));
	setupGround(&world, &objects);
	setupCharacter(&world, &objects);
	setupWater(&world, &objects);

#if 0
	btCompoundShape *compoundShape = new btCompoundShape();

	// Add shapes to the compound shape
	for (int i = 0; i < 2; ++i) {
		btBoxShape *box = new btBoxShape(btVector3(1, 1, 1));
		btTransform transform;
		transform.setIdentity();
		transform.setOrigin(btVector3(0, 1 - i, 0));
		compoundShape->addChildShape(transform, box);
	}

	btDefaultMotionState *compoundMotionState = new btDefaultMotionState(
		btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 10, 0)));
	btScalar mass = 5.0;
	btVector3 inertia(0, 0, 0);
	compoundShape->calculateLocalInertia(mass, inertia);
	btRigidBody::btRigidBodyConstructionInfo compoundRigidBodyCI(
		mass, compoundMotionState, compoundShape, inertia);
	btRigidBody *compoundRigidBody = new btRigidBody(compoundRigidBodyCI);
	world.addRigidBody(compoundRigidBody);
#endif

	// Simulation
	btVector3 velocity(0, 0, 0);
	for (int i = 0; i < 300; i++) {
		world.stepSimulation(1.f / 60.f, 10);
		showContacts(&world, &objects);

#if 0
		btTransform transform;
		compoundRigidBody->getMotionState()->getWorldTransform(
			transform);
		std::cout << "Step " << i
			  << ": Position = " << transform.getOrigin().getX()
			  << ", " << transform.getOrigin().getY() << ", "
			  << transform.getOrigin().getZ() << std::endl;
		std::cout << "water overlaps:"
			  << objects.waterBody->getOverlappingPairCache()
				     ->getNumOverlappingPairs()
			  << "\n";
#endif
		btTransform transform;
		transform = objects.characterBody->getWorldTransform();
		std::cout << "Step " << i << " ";
		std::cout << "Character: "
			  << "Position = " << transform.getOrigin().getX()
			  << ", " << transform.getOrigin().getY() << ", "
			  << transform.getOrigin().getZ() << std::endl;
		transform = objects.waterBody->getWorldTransform();
		std::cout << "Water: "
			  << "Position = " << transform.getOrigin().getX()
			  << ", " << transform.getOrigin().getY() << ", "
			  << transform.getOrigin().getZ() << std::endl;
		std::cout << "water overlaps:"
			  << objects.waterBody->getOverlappingPairCache()
				     ->getNumOverlappingPairs()
			  << "\n";
		dumpCompoundShape("Character", objects.characterBody);
		dumpCompoundShape("Water", objects.waterBody);
		btCollisionObjectWrapper obA(
			NULL, objects.waterBody->getCollisionShape(),
			objects.waterBody,
			objects.waterBody->getWorldTransform(), -1, 0);
		btCollisionObjectWrapper obB(
			NULL, objects.characterBody->getCollisionShape(),
			objects.characterBody,
			objects.characterBody->getWorldTransform(), -1, 0);
		std::cout << __func__ << ": call findAlgorithm: ";
		btCollisionAlgorithm *algorithm = dispatch->findAlgorithm(
			&obA, &obB, NULL, BT_CONTACT_POINT_ALGORITHMS);
		std::cout << "found algorithm: " << algorithm << "\n";
		if (!algorithm)
			continue;
		std::cout << "algorithm: " << algorithm << "\n";
		DeepPenetrationContactResultCallback contactPointResult(&obA,
									&obB);
		std::cout << "process collision\n";
		algorithm->processCollision(&obA, &obB, world.getDispatchInfo(),
					    &contactPointResult);
		algorithm->~btCollisionAlgorithm();
		dispatch->freeCollisionAlgorithm(algorithm);
		if (contactPointResult.hasHit()) {
			std::cout << "InWater!!1\n";
		}
		velocity += btVector3(0, -9.8, 0) * 1.0f / 16.0f;
		objects.characterBody->getWorldTransform().getOrigin() += velocity * 1.0f / 16.0f;
		std::cout << "process collision done\n";
	}
}