#include "physics.h"

#include "internal.h"
#include "internal/physics.h"

#include <glm/glm.hpp>

namespace kek::Physics {

void init() {
	kekData.physics = new PhysicsData();

	btDefaultCollisionConfiguration *collisionConf = new btDefaultCollisionConfiguration();
	btCollisionDispatcher *dispatcher = new btCollisionDispatcher(collisionConf);
	btBroadphaseInterface *overlappingPairCache = new btDbvtBroadphase();
	btSequentialImpulseConstraintSolver *solver = new btSequentialImpulseConstraintSolver();
	kekData.physics->world = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache, solver, collisionConf);
	kekData.physics->world->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
	kekData.physics->world->setGravity(btVector3(0, -10, 0));
}

void destroy() {
	for(int i = kekData.physics->world->getNumCollisionObjects() - 1; i >= 0; i--) {
		btCollisionObject *obj = kekData.physics->world->getCollisionObjectArray()[i];
		btRigidBody *rigid = btRigidBody::upcast(obj);
		if(obj->getCollisionShape()) delete obj->getCollisionShape();
		if(rigid && rigid->getMotionState()) delete rigid->getMotionState();
		kekData.physics->world->removeCollisionObject(obj);
		delete obj;
	}
	delete kekData.physics;
}

void step(float deltaT) {
	for(GameObject *obj : kekData.activeScene->objects) {
		if(obj->physics) {
			obj->physics->body->getWorldTransform().setOrigin(fromGLM(obj->getPosition()));
		}
	}

	kekData.player->physics->jumpCollider->getWorldTransform().setOrigin(Physics::fromGLM(kekData.player->getFootPosition() + glm::vec3(0, KEK_PLAYER_RADIUS / 2, 0)));

	kekData.physics->world->stepSimulation(deltaT, 100);

	kekData.player->onGround = kekData.player->physics->jumpCollider->getNumOverlappingObjects() > 1; // TODO: improve

	if(!kekData.player->noclip) {
		kekData.activeCamera->moveTo(kekData.player->getEyePosition());
	}
}

glm::vec3 toGLM(btVector3 vec) {
	return glm::vec3(vec.x(), vec.y(), vec.z());
}

btVector3 fromGLM(glm::vec3 vec) {
	return btVector3(vec.x, vec.y, vec.z);
}

glm::quat toGLM(btQuaternion quat) {
	return glm::quat(quat.w(), quat.x(), quat.y(), quat.z());
}

btQuaternion fromGLM(glm::quat quat) {
	return btQuaternion(quat.x, quat.y, quat.z, quat.w);
}

}
