#include "engine/core.h"
#include "engine/log.h"
#include "engine/engine.h"
#include "engine/ientity.h"
#include "engine/camera.h"
#include "engine/physics/physicsworld.h"
#include "engine/physics/physicsdebugdraw.h"
#include "engine/physics/rigidbody.h"

#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>

PhysicsWorld* g_PhysicsWorld = nullptr;

static void InternalTickCallback(btDynamicsWorld* world, btScalar timeStep)
{
	SDL_assert(world);
	SDL_assert(world->getWorldUserInfo());
	static_cast<PhysicsWorld*>(world->getWorldUserInfo())->InternalTick();
}

PhysicsWorld::PhysicsWorld()
{
	m_debugDraw = false;

	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
	m_overlappingPairCache = new btDbvtBroadphase();
	m_solver = new btSequentialImpulseConstraintSolver();
	m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);
	m_btGhostPairCallback = new btGhostPairCallback();

	m_world->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(m_btGhostPairCallback);
	m_world->setInternalTickCallback(InternalTickCallback, this);
	m_world->setDebugDrawer(&m_physicsDebugDraw);

	m_accumulatedTime = 0.0f;
	m_stepTime = (1.0f / 30.0f); // 30 fps

	// set the standart gravity
	//m_world->setGravity(btVector3(0, 0, 0));
	//getWorld()->setGravity(btVector3(0.0, -15.0, 0.0));
}

PhysicsWorld::~PhysicsWorld()
{
	m_world->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(nullptr);

	// delete ghost pair callback
	delete m_btGhostPairCallback;

	// delete dynamics world
	delete m_world;

	// delete solver
	delete m_solver;

	// delete broadphase
	delete m_overlappingPairCache;

	// delete dispatcher
	delete m_dispatcher;

	// delete collision configuration
	delete m_collisionConfiguration;
}

void PhysicsWorld::AddRigidBody(RigidBody* body)
{
	SDL_assert(body->GetSDKBody() && "RigidBody is not properly initialized");
	m_world->addRigidBody(body->GetSDKBody());

	m_rigidbodies.push_back(body);
}

void PhysicsWorld::RemoveRigidBody(RigidBody* body)
{
	SDL_assert(body->GetSDKBody() && "RigidBody is not properly initialized");
	m_world->removeRigidBody(body->GetSDKBody());

	auto it = std::find(m_rigidbodies.begin(), m_rigidbodies.end(), body);
	if (it != m_rigidbodies.end())
		m_rigidbodies.erase(it);
}

const std::vector<RigidBody*>& PhysicsWorld::GetRigidBodies()
{
	return m_rigidbodies;
}

void PhysicsWorld::Step(float delta)
{
		m_world->stepSimulation(delta);
	//	m_world->stepSimulation(delta, 12, m_stepTime);
#if 0
	if (delta < 0.01f)
	{
		m_accumulatedTime += delta;
		if (m_accumulatedTime > m_stepTime)
		{
			m_world->stepSimulation(m_stepTime);
			m_accumulatedTime -= m_stepTime;
		}
	}
	else
	{
		m_world->stepSimulation(delta);
	}
#endif

	//m_debugDraw = true;
	if (m_debugDraw)
	{
		//DebugDrawTriggers();

		int debugDrawMode = 0;
		debugDrawMode |= btIDebugDraw::DBG_DrawAabb;
		debugDrawMode |= btIDebugDraw::DBG_DrawWireframe;
		// debugDrawMode |= btIDebugDraw::DBG_DrawContactPoints;
		m_physicsDebugDraw.setDebugMode(debugDrawMode);

		m_world->debugDrawWorld();
	}

	/*if (m_world)
		m_world->stepSimulation(1 / 60.0f);

	int debugDrawMode = 0;
	debugDrawMode |= btIDebugDraw::DBG_DrawAabb;
	debugDrawMode |= btIDebugDraw::DBG_DrawWireframe;
	debugDrawMode |= btIDebugDraw::DBG_DrawContactPoints;
	m_physicsDebugDraw.setDebugMode(debugDrawMode);

	m_world->debugDrawWorld();*/
}

void PhysicsWorld::ToggleDebugDraw()
{
	m_debugDraw = !m_debugDraw;
}

void PhysicsWorld::InternalTick()
{
	int numManifolds = m_world->getDispatcher()->getNumManifolds();
	for (int i = 0; i < numManifolds; i++)
	{
		btPersistentManifold* contactManifold = m_world->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject* obA = const_cast<btCollisionObject*>(contactManifold->getBody0());
		btCollisionObject* obB = const_cast<btCollisionObject*>(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;

				//RigidBody* rbA = static_cast<RigidBody*>(obA->getUserPointer());
				//RigidBody* rbB = static_cast<RigidBody*>(obB->getUserPointer());
				//if (!rbA || !rbB)
				//	continue;

				//SDL_assert(rbA);
				//SDL_assert(rbB);



				IEntityBase* entA = static_cast<IEntityBase*>(obA->getUserPointer());
				IEntityBase* entB = static_cast<IEntityBase*>(obB->getUserPointer());
				if (!entA || !entB)
					continue;

				if (entA)
					entA->OnCollide(entB);

				if (entB)
					entB->OnCollide(entA);
			

				/*if (TriggerComponent* trigger = rbA->GetEntity()->GetComponent<TriggerComponent>())
				{
					trigger->OnCollide(rbA, rbB);
				}
				else if (TriggerComponent* trigger = rbB->GetEntity()->GetComponent<TriggerComponent>())
				{
					trigger->OnCollide(rbB, rbA);
				}*/
			}
		}
	}
}

void PhysicsWorld::AddCollisionModel(StaticMeshVertex* vertices, size_t verticesCount, uint32_t* indices, size_t indicesCount)
{
	int index = (int)m_collisionModels.size();
	m_collisionModels.resize((size_t)index + 1);

	// Check for is indices are 32 bits
	bool is32Bits = true;// sizeof(index_t) == sizeof(uint32_t);

	m_collisionModels[index].triangleMesh = new btTriangleMesh(is32Bits);
	m_collisionModels[index].triangleMesh->preallocateVertices((int)verticesCount);
	m_collisionModels[index].triangleMesh->preallocateIndices((int)indicesCount);

	// Initialize triangle mesh 
	uint32_t trianglesCount = 0;
	for (uint32_t n = 0; n < indicesCount; n += 3) {
		m_collisionModels[index].triangleMesh->addTriangle(
			glmVectorToBt(vertices[indices[n]].position),
			glmVectorToBt(vertices[indices[n + 1]].position),
			glmVectorToBt(vertices[indices[n + 2]].position),
			true);

		trianglesCount++;
	}

	// Create shape
	m_collisionModels[index].shape = new btBvhTriangleMeshShape(m_collisionModels[index].triangleMesh, true);

	// Create collision body
	m_collisionModels[index].object = new btCollisionObject();
	m_collisionModels[index].object->setCollisionShape(m_collisionModels[index].shape);
	//m_collisionModels[index].object->setUserPointer(&m_collisionModels[index]);

	// Add to scene
	m_world->addCollisionObject(m_collisionModels[index].object);

	// Report
	Msg("PhysicsWorld::AddCollisionModel: %i triangles %i bytes", trianglesCount, trianglesCount * sizeof(btVector3));
}

void PhysicsWorld::Reset()
{
	int numModels = (int)m_collisionModels.size();
	for (int i = 0; i < numModels; i++)
	{
		if (m_collisionModels[i].object)
		{
			// remove object from world
			m_world->removeCollisionObject(m_collisionModels[i].object);

			// reset shape
			m_collisionModels[i].object->setCollisionShape(nullptr);

			// delete
			delete m_collisionModels[i].object;
			m_collisionModels[i].object = nullptr;
		}

		if (m_collisionModels[i].shape)
		{
			delete m_collisionModels[i].shape;
			m_collisionModels[i].shape = nullptr;
		}

		if (m_collisionModels[i].triangleMesh)
		{
			delete m_collisionModels[i].triangleMesh;
			m_collisionModels[i].triangleMesh = nullptr;
		}
	}

	Msg("PhysicsWorld: Destroyed %d static collision models", numModels);

	m_collisionModels.clear();
}

//
ClosestRayResultCallback::ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, IEntityBase* entity) :
	btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
	m_entity(entity)
{
}

ClosestRayResultCallback::~ClosestRayResultCallback()
{
	m_entity = nullptr;
}

bool ClosestRayResultCallback::needsCollision(btBroadphaseProxy* proxy0) const
{
	bool needsCollision = inherited::needsCollision(proxy0);

	btCollisionObject* collisionObject = (btCollisionObject*)proxy0->m_clientObject;
	if (m_entity && collisionObject->getUserPointer() == m_entity)
		return false;

	return needsCollision;
}