#include "engine/core.h"
#include "engine/engine.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>

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 1
	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);

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

