#include "physics.hpp"

#include "components.hpp"
#include "dynamic-array.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;
};
DynArray<AllocedData> *bulletAllocs;

btDefaultCollisionConfiguration *btConfiguration = nullptr;
btCollisionDispatcher *btDispatcher = nullptr;
btBroadphaseInterface *btBroadphase = nullptr;
btConstraintSolver *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);
	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);
	pk_delete_bkt(memBlock, arr[index].size, MemBkt_Bullet);
	bulletAllocs->Remove(index);
}
void *pke_btAllocFunc(size_t size) {
	void *ptr = pk_new_bkt(size, PK_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);
	pk_delete_bkt(memBlock, arr[index].size, MemBkt_Bullet);
	bulletAllocs->Remove(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_bucket_create("physics", PK_DEFAULT_BUCKET_SIZE, false);
	bulletAllocs = pk_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 = pk_new<btDefaultCollisionConfiguration>(MemBkt_Bullet);
	btDispatcher = pk_new<btCollisionDispatcher>(MemBkt_Bullet);
	new (btDispatcher) btCollisionDispatcher(btConfiguration);

	btBroadphase = pk_new<btDbvtBroadphase>(MemBkt_Bullet);
#if 1
	btHashedOverlappingPairCache *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_bucket_destroy(MemBkt_Bullet);
}