#include "physics.hpp"

#include "components.hpp"
#include "ecs.hpp"
#include "game-settings.hpp"
#include "pk.h"

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

TypeSafeInt_B(PhysicsCollision);

struct pk_membucket *MemBkt_Bullet = nullptr;
btDiscreteDynamicsWorld *BtDynamicsWorld = nullptr;

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

btHashedOverlappingPairCache *overlappingPairCache = nullptr;
btDefaultCollisionConfiguration *btConfiguration = nullptr;
btCollisionDispatcher *btDispatcher = nullptr;
btDbvtBroadphase *btBroadphase = nullptr;
btSequentialImpulseConstraintSolver *btSolver = nullptr;

struct EntityCollision {
	CompInstance *a, *b;
};
pk_arr_t<EntityCollision> collisionsThisTick{};

void *pke_btAlignedAllocFunc(size_t size, int alignment) {
	void *ptr = pk_new_bkt(size, alignment, MemBkt_Bullet);
	pk_arr_append_t(&bulletAllocs, {ptr, size});
	return ptr;
}
void pke_btAlignedFreeFunc(void *memBlock) {
	int64_t index = -1;
	uint32_t i, count;
	count = bulletAllocs.next;
	for (i = 0; i < count; ++i) {
		if (bulletAllocs[i].data == memBlock) {
			index = i;
			break;
		}
	}
	assert(index != -1);
	pk_delete_bkt(memBlock, bulletAllocs[index].size, MemBkt_Bullet);
	pk_arr_remove_at(&bulletAllocs, index);
}
void *pke_btAllocFunc(size_t size) {
	// 2025-06-05 JCB
	// I have no idea what minimum alignment should be.
	// However, there *is* an aligned func defined above.
	// Maybe 1 is fine?
	void *ptr = pk_new_bkt(size, 1, MemBkt_Bullet);
	pk_arr_append_t(&bulletAllocs, {ptr, size});
	return ptr;
}
void pke_btFreeFunc(void *memBlock) {
	int64_t index = -1;
	uint32_t i, count;
	count = bulletAllocs.next;
	for (i = 0; i < count; ++i) {
		if (bulletAllocs[i].data == memBlock) {
			index = i;
			break;
		}
	}
	assert(index != -1);
	pk_delete_bkt(memBlock, bulletAllocs[index].size, MemBkt_Bullet);
	pk_arr_remove_at(&bulletAllocs, index);
}

struct CollisionHandlerStruct : public btOverlapFilterCallback {
	~CollisionHandlerStruct() override {}
	bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override {
		EntityCollision col;
		auto collided = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) |
			(proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
		if (collided) {
			const auto *col0 = static_cast<btCollisionObject *>(proxy0->m_clientObject);
			const auto *col1 = static_cast<btCollisionObject *>(proxy1->m_clientObject);
			if (col0 && col1) {
				CompInstance *ent0 = reinterpret_cast<CompInstance *>(col0->getUserPointer());
				CompInstance *ent1 = reinterpret_cast<CompInstance *>(col1->getUserPointer());
				if (ent0 != nullptr && ent1 != nullptr) {
					col.a = ent0;
					col.b = ent1;
					pk_arr_append(&collisionsThisTick, &col);
				}
			}
		}
		return collided;
	}
} collisionHandlerStruct;

void Physics_Init() {
	MemBkt_Bullet = pk_mem_bucket_create("physics", PK_MEM_DEFAULT_BUCKET_SIZE, PK_MEMBUCKET_FLAG_NONE);
	bulletAllocs.bkt = MemBkt_Bullet;
	pk_arr_reserve(&bulletAllocs, 1024);
	btAlignedAllocSetCustom(pke_btAllocFunc, pke_btFreeFunc);
	btAlignedAllocSetCustomAligned(pke_btAlignedAllocFunc, pke_btAlignedFreeFunc);

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

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

	BtDynamicsWorld = pk_new<btDiscreteDynamicsWorld>(MemBkt_Bullet);
	new (BtDynamicsWorld) btDiscreteDynamicsWorld(btDispatcher, btBroadphase, btSolver, btConfiguration);
}

int32_t Physics_Tick(double delta) {
	if (pkeSettings.isSimulationPaused == true)
		return 0;
	pk_arr_clear(&collisionsThisTick);
	auto tickCount = BtDynamicsWorld->stepSimulation(delta, 1);
	for (long i = 0; i < collisionsThisTick.next; ++i) {
		ECS_HandleCollision(collisionsThisTick[i].a, collisionsThisTick[i].b);
	}
	return tickCount;
}

void Physics_Teardown() {
	pk_arr_reset(&collisionsThisTick);
	if (BtDynamicsWorld != nullptr)
		pk_delete<btDiscreteDynamicsWorld>(BtDynamicsWorld, MemBkt_Bullet);
	if (overlappingPairCache != nullptr)
		pk_delete<btHashedOverlappingPairCache>(overlappingPairCache, MemBkt_Bullet);
	if (btConfiguration != nullptr)
		pk_delete<btDefaultCollisionConfiguration>(btConfiguration, MemBkt_Bullet);
	if (btSolver != nullptr)
		pk_delete<btSequentialImpulseConstraintSolver>(btSolver, MemBkt_Bullet);
	if (btDispatcher != nullptr)
		pk_delete<btCollisionDispatcher>(btDispatcher, MemBkt_Bullet);
	if (btBroadphase != nullptr)
		pk_delete<btDbvtBroadphase>(btBroadphase, MemBkt_Bullet);
	btConfiguration = nullptr;
	btSolver = nullptr;
	btBroadphase = nullptr;
	btDispatcher = nullptr;
	BtDynamicsWorld = nullptr;
	// TODO should we manually delete each bullet alloc?
	// or just drop?
	pk_arr_reset(&bulletAllocs);
	pk_mem_bucket_destroy(MemBkt_Bullet);
}