#include "physics.hpp"

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

#include <btBulletDynamicsCommon.h>
#include <LinearMath/btAlignedAllocator.h>

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

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