#include "physics.hpp"

#include "components.hpp"
#include "dynamic-array.hpp"
#include "game-settings.hpp"

#include <LinearMath/btAlignedAllocator.h>
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/BroadphaseCollision/btOverlappingPairCache.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>

TypeSafeInt_B(PhysicsCollision);

MemBucket *MemBkt_Bullet = nullptr;
btDiscreteDynamicsWorld *BtDynamicsWorld = nullptr;

struct AllocedData {
	void *data;
	std::size_t size;
};
DynArray<AllocedData> *bulletAllocs;

btDefaultCollisionConfiguration *btConfiguration = nullptr;
btCollisionDispatcher *btDispatcher = nullptr;
btBroadphaseInterface *btBroadphase = nullptr;
btConstraintSolver *btSolver = nullptr;

void *pke_btAlignedAllocFunc(size_t size, int alignment) {
	void *ptr = Pke_New(size, alignment, MemBkt_Bullet);
	bulletAllocs->Push({ptr, size});
	return ptr;
}
void pke_btAlignedFreeFunc(void *memBlock) {
	auto &arr = *bulletAllocs;
	auto count = arr.Count();
	long index = -1;
	for (long i = 0; i < count; ++i) {
		if (arr[i].data == memBlock) {
			index = i;
			break;
		}
	}
	assert(index != -1);
	Pke_Delete(const_cast<void *>(memBlock), arr[index].size, MemBkt_Bullet);
	bulletAllocs->Remove(index);
}
void *pke_btAllocFunc(size_t size) {
	void *ptr = Pke_New(size, MINIMUM_ALIGNMENT, MemBkt_Bullet);
	bulletAllocs->Push({ptr, size});
	return ptr;
}
void pke_btFreeFunc(void *memBlock) {
	auto &arr = *bulletAllocs;
	auto count = arr.Count();
	long index = -1;
	for (long i = 0; i < count; ++i) {
		if (arr[i].data == memBlock) {
			index = i;
			break;
		}
	}
	assert(index != -1);
	Pke_Delete(const_cast<void *>(memBlock), arr[index].size, MemBkt_Bullet);
	bulletAllocs->Remove(index);
}

struct CollisionHandlerStruct : public btOverlapFilterCallback {
	~CollisionHandlerStruct() override {}
	bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override {
		return (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) ||
			(proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
	}
} collisionHandlerStruct;

void Physics_Init() {
	MemBkt_Bullet = Pke_BeginTransientBucket();
	bulletAllocs = Pke_New<DynArray<AllocedData>>(MemBkt_Bullet);
	new (bulletAllocs) DynArray<AllocedData>(MemBkt_Bullet);
	bulletAllocs->Reserve(1024);
	btAlignedAllocSetCustom(pke_btAllocFunc, pke_btFreeFunc);
	btAlignedAllocSetCustomAligned(pke_btAlignedAllocFunc, pke_btAlignedFreeFunc);

	btConfiguration = Pke_New<btDefaultCollisionConfiguration>(MemBkt_Bullet);
	btDispatcher = Pke_New<btCollisionDispatcher>(MemBkt_Bullet);
	new (btDispatcher) btCollisionDispatcher(btConfiguration);

	btBroadphase = Pke_New<btDbvtBroadphase>(MemBkt_Bullet);
#if 1
	btHashedOverlappingPairCache *overlappingPairCache = Pke_New<btHashedOverlappingPairCache>(MemBkt_Bullet);
	overlappingPairCache->setOverlapFilterCallback(&collisionHandlerStruct);
	new (btBroadphase) btDbvtBroadphase(overlappingPairCache);
#else
	new (btBroadphase) btDbvtBroadphase();
#endif
	btSolver = Pke_New<btSequentialImpulseConstraintSolver>(MemBkt_Bullet);

	BtDynamicsWorld = Pke_New<btDiscreteDynamicsWorld>(MemBkt_Bullet);
	new (BtDynamicsWorld) btDiscreteDynamicsWorld(btDispatcher, btBroadphase, btSolver, btConfiguration);
	auto grav = BtDynamicsWorld->getGravity();
	BtDynamicsWorld->setGravity(btVector3(grav.getX(), grav.getZ(), grav.getY()));
}

int32_t Physics_Tick(double delta) {
	if (pkeSettings.isSimulationPaused == true)
		return 0;
	return BtDynamicsWorld->stepSimulation(delta, 0);
}

void Physics_Teardown() {
	Pke_EndTransientBucket(MemBkt_Bullet);
}