#include "../../include/Physics/Physics.hpp"

#include <btBulletDynamicsCommon.h>

// TODO: Figure out how to process physics in another thread. Should it be done simply by
// calling stepSimulation in another thread?

static void physics_update_callback(btDynamicsWorld *dynamics_world, btScalar time_step) {
    btDispatcher *dispatcher = dynamics_world->getDispatcher();
    int num_manifolds = dispatcher->getNumManifolds();
    for(int i = 0; i < num_manifolds; ++i) {
        btPersistentManifold *contact_manifold = dispatcher->getManifoldByIndexInternal(i);
        {
            // The bodies that collided
            const btCollisionObject *obj_this = contact_manifold->getBody0();
            const btCollisionObject *obj_other = contact_manifold->getBody1();
        }
        int num_contacts = contact_manifold->getNumContacts();
        for(int j = 0; j < num_contacts; ++j) {
            btManifoldPoint &pt = contact_manifold->getContactPoint(j);
            const btVector3 &pt_this = pt.getPositionWorldOnA();
            const btVector3 &pt_other = pt.getPositionWorldOnB();
            const btVector3 &normal_on_other = pt.m_normalWorldOnB;
        }
    }
}

namespace amalgine {
    Physics::Physics() {
        btDefaultCollisionConfiguration *collision_configuration = new btDefaultCollisionConfiguration();
        btCollisionDispatcher *dispatcher = new btCollisionDispatcher(collision_configuration);
        btBroadphaseInterface *overlapping_pair_cache = new btDbvtBroadphase();
        btSequentialImpulseConstraintSolver *solver = new btSequentialImpulseConstraintSolver();
        btDiscreteDynamicsWorld *dynamics_world = new btDiscreteDynamicsWorld(dispatcher, overlapping_pair_cache, solver, collision_configuration);
        const float gravity = -10.0f;
        dynamics_world->setGravity(btVector3(0.0f, gravity, 0.0f));
        dynamics_world->setInternalTickCallback(physics_update_callback);
        //dynamics_world->debugDrawWorld();
    }
}