#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "bucketed-array.hpp"
#include "camera.hpp"
#include "ecs.hpp"
#include "math-helpers.hpp"
#include "physics.hpp"

PkeCamera NullCamera {
	.camHandle = CameraHandle_MAX,
	.type = PKE_CAMERA_TYPE_ORTHOGONAL,
	.view = PKE_CAMERA_VIEW_TARGET,
	.stale = PKE_CAMERA_STALE_ALL,
	.phys = {
		.inst = nullptr,
		.targetInst = nullptr,
		.constraint = nullptr,
	},
};
PkeCamera *ActiveCamera = &NullCamera;

const pk_handle_item_index_T MAX_CAMERAS_PER_BUCKET = 32;
BucketContainer<PkeCamera, CameraHandle> Camera_BucketContainer{};

btSphereShape CameraShape{1.f};

PkeCamera &PkeCamera_Register_Inner(PkeCamera &cam, const InstPos &instPos) {
	btVector3 gravity(0.f, 0.f, 0.f);

	cam.phys.inst->physicsLayer = PhysicsCollision{0};
	cam.phys.inst->physicsMask = PhysicsCollision{0};

	btVector3 localInertia(0, 0, 0);
	CameraShape.calculateLocalInertia(instPos.mass, localInertia);
	cam.phys.inst->bt.motionState = pk_new<btDefaultMotionState>(MemBkt_Bullet);
	new (cam.phys.inst->bt.motionState) btDefaultMotionState(instPos.posRot);

	cam.phys.inst->bt.rigidBody = pk_new<btRigidBody>(MemBkt_Bullet);
	new (cam.phys.inst->bt.rigidBody) btRigidBody(instPos.mass, cam.phys.inst->bt.motionState, &CameraShape, localInertia);

	cam.phys.inst->bt.rigidBody->setLinearVelocity(btVector3(0,0,0));
	cam.phys.inst->bt.rigidBody->setAngularVelocity(btVector3(0,0,0));
	cam.phys.inst->bt.rigidBody->getCollisionShape()->setLocalScaling(instPos.scale);
	BtDynamicsWorld->addRigidBody(cam.phys.inst->bt.rigidBody);
	cam.phys.inst->bt.rigidBody->setGravity(gravity);
	cam.phys.inst->bt.rigidBody->getBroadphaseProxy()->m_collisionFilterGroup = static_cast<PhysicsCollision_T>(cam.phys.inst->physicsLayer);
	cam.phys.inst->bt.rigidBody->getBroadphaseProxy()->m_collisionFilterMask = static_cast<PhysicsCollision_T>(cam.phys.inst->physicsMask);
	cam.phys.inst->bt.rigidBody->setUserPointer(reinterpret_cast<void *>(cam.phys.inst));

	return cam;
}

PkeCamera &PkeCamera_Register(const InstPos &instPos) {
	CameraHandle cameraHandle{Buckets_NewHandle(Camera_BucketContainer)};

	auto &cam = Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
	new (&cam) PkeCamera{};

	ECS_CreateEntity(&cam, nullptr);
	cam.camHandle = cameraHandle;
	cam.phys.inst = ECS_CreateInstance(&cam, nullptr);

	return PkeCamera_Register_Inner(cam, instPos);
}

PkeCamera *PkeCamera_Get(CameraHandle cameraHandle) {
	assert(cameraHandle != CameraHandle_MAX);
	return &Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
}

PkeCamera *PkeCamera_Get(EntityHandle handle) {
	assert(handle != EntityHandle_MAX);
	for (pk_handle_bucket_index_T b = 0; b <= Camera_BucketContainer.pkeHandle.bucketIndex; ++b) {
		auto &bkt = Camera_BucketContainer.buckets[b];
		long itemCount = Camera_BucketContainer.pkeHandle.bucketIndex == b ? Camera_BucketContainer.pkeHandle.itemIndex : Camera_BucketContainer.limits.itemIndex;
		for (pk_handle_item_index_T i = 0; i < itemCount; ++i) {
			auto &cam = bkt[i];
			if (cam.handle == handle)
				return &Camera_BucketContainer.buckets[cam.camHandle.bucketIndex][cam.camHandle.itemIndex];
		}
	}
	return nullptr;
}

void PkeCamera_AttachToInstance(CameraHandle cameraHandle, CompInstance *inst) {
	assert(cameraHandle != CameraHandle_MAX);
	auto &cam = Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];

	if (cam.phys.constraint != nullptr && cam.phys.constraint != CAFE_BABE(btTypedConstraint)) {
		CompInstance *activeInst = reinterpret_cast<CompInstance *>(cam.phys.constraint->getRigidBodyB().getUserIndex());
		PkeCamera_DetachFromInstance(cameraHandle, activeInst);
	}

	btVector3 cameraOffset(0.f, -10.f, -10.f);

	btTransform trfm;
	inst->bt.motionState->getWorldTransform(trfm);
	trfm.setOrigin(trfm.getOrigin() + cameraOffset);

	cam.phys.targetInst = inst;
	cam.phys.inst->bt.motionState->setWorldTransform(trfm);
	cam.phys.inst->bt.rigidBody->setWorldTransform(trfm);
	cam.phys.inst->bt.rigidBody->setLinearVelocity(btVector3(0,0,0));
	cam.phys.inst->bt.rigidBody->setAngularVelocity(btVector3(0,0,0));
	cam.phys.inst->bt.rigidBody->activate();

	assert(cam.phys.constraint == nullptr || cam.phys.constraint == CAFE_BABE(btTypedConstraint));
	cam.phys.constraint = pk_new<btTypedConstraint>(MemBkt_Bullet);
	new (cam.phys.constraint) btPoint2PointConstraint(*cam.phys.inst->bt.rigidBody, *inst->bt.rigidBody, btVector3(0.f, -1.f, -1.f), cameraOffset);
	BtDynamicsWorld->addConstraint(cam.phys.constraint);

	cam.view = PKE_CAMERA_VIEW_TARGET;
	cam.stale = PKE_CAMERA_STALE_POSROT;
}

void PkeCamera_DetachFromInstance(CameraHandle cameraHandle, CompInstance *inst) {
	assert(cameraHandle != CameraHandle_MAX);
	auto &cam = Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
	BtDynamicsWorld->removeConstraint(cam.phys.constraint);
	pk_delete<btTypedConstraint>(cam.phys.constraint, MemBkt_Bullet);
	cam.phys.constraint = CAFE_BABE(btTypedConstraint);
	cam.stale = PKE_CAMERA_STALE_POSROT;
}

void PkeCamera_SetPrimary(CameraHandle cameraHandle) {
	assert(cameraHandle != CameraHandle_MAX);
	auto &primaryCam = Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
	for (pk_handle_bucket_index_T b = 0; b <= Camera_BucketContainer.pkeHandle.bucketIndex; ++b) {
		auto &bkt = Camera_BucketContainer.buckets[b];
		long itemCount = Camera_BucketContainer.pkeHandle.bucketIndex == b ? Camera_BucketContainer.pkeHandle.itemIndex : Camera_BucketContainer.limits.itemIndex;
		for (pk_handle_item_index_T i = 0; i < itemCount; ++i) {
			auto *cam = &bkt[i];
			if (cam->parentHandle != primaryCam.parentHandle) {
				continue;
			}
			cam->isPrimary = cam->camHandle == cameraHandle;
		}
	}
}

void PkeCamera_Destroy(CameraHandle cameraHandle) {
	assert(cameraHandle != CameraHandle_MAX);
	auto *camPtr = &Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
	auto &cam = *camPtr;

	if (cam.phys.constraint != nullptr && cam.phys.constraint != CAFE_BABE(btTypedConstraint)) {
		// reminder: this is not currently handled by ECS
		BtDynamicsWorld->removeConstraint(cam.phys.constraint);
		pk_delete<btTypedConstraint>(cam.phys.constraint, MemBkt_Bullet);
	}

	ECS_MarkForRemoval(camPtr);
	cam.camHandle = CameraHandle_MAX;
	cam.type = PkeCameraType_MAX;
	cam.view = PkeCameraView_MAX;
	cam.stale = PkeCameraStaleFlags_MAX;
	cam.phys.inst = CAFE_BABE(CompInstance);
	cam.phys.targetInst = CAFE_BABE(CompInstance);
	cam.phys.constraint = CAFE_BABE(btTypedConstraint);
}

int64_t PkeCamera_GetBucketCount() {
	return Camera_BucketContainer.pkeHandle.bucketIndex + 1;
}

PkeCamera *PkeCamera_GetCameras(int64_t bucketIndex, int64_t &count) {
	if (Camera_BucketContainer.pkeHandle.bucketIndex == bucketIndex) {
		count = Camera_BucketContainer.pkeHandle.itemIndex;
	} else {
		count = MAX_CAMERAS_PER_BUCKET;
	}
	return Camera_BucketContainer.buckets[bucketIndex];
}

void PkeCamera_Init() {
	Buckets_Init(Camera_BucketContainer, MAX_CAMERAS_PER_BUCKET);
	NullCamera.phys.inst = pk_new<CompInstance>();
	InstPos instPos{
		.scale = btVector3(1.f, 1.f, 1.f),
		.mass = 1.f,
	};
	PkeCamera_Register_Inner(NullCamera, instPos);
}

void PkeCamera_Teardown() {
	Buckets_Destroy(Camera_BucketContainer);
	BtDynamicsWorld->removeRigidBody(NullCamera.phys.inst->bt.rigidBody);
	pk_delete<btDefaultMotionState>(NullCamera.phys.inst->bt.motionState, MemBkt_Bullet);
	pk_delete<btRigidBody>(NullCamera.phys.inst->bt.rigidBody, MemBkt_Bullet);
	NullCamera.phys.inst->bt.motionState = CAFE_BABE(btDefaultMotionState);
	NullCamera.phys.inst->bt.rigidBody = CAFE_BABE(btRigidBody);
	pk_delete<CompInstance>(NullCamera.phys.inst);
	NullCamera.phys.inst = CAFE_BABE(CompInstance);
}

void PkeCamera_Tick(double delta) {
	/* 2024-01-16 - JCB - This seems excessive to loop through every camera and do this.
	 * I think this could be simplified, but it might be premature optimization.
	 * Why we're looping all:
	 * - To avoid any weird scenarios where the active camera is changed and
	 *   1 frame the camera is in the wrong position.
	 * - To prevent various 'saves' that save camera positions from saving bad positional data
	 * It might be possible to handle these two scenarios explicitly, or it
	 *  could be that removing pos and rot from the camera would make this unnecessary?
	 *  See the camera serializer for more.
	 */
	for (pk_handle_bucket_index_T b = 0; b <= Camera_BucketContainer.pkeHandle.bucketIndex; ++b) {
		auto &bkt = Camera_BucketContainer.buckets[b];
		long itemCount = Camera_BucketContainer.pkeHandle.bucketIndex == b ? Camera_BucketContainer.pkeHandle.itemIndex : Camera_BucketContainer.limits.itemIndex;
		for (pk_handle_item_index_T i = 0; i < itemCount; ++i) {
			auto &cam = bkt[i];
			if (cam.handle == EntityHandle_MAX) {
				continue;
			}
			if (cam.phys.inst != nullptr && cam.phys.inst != CAFE_BABE(CompInstance) && cam.phys.inst->isNeedingUpdated == true) {
				cam.stale = cam.stale | PKE_CAMERA_STALE_POSROT;
				cam.phys.inst->isNeedingUpdated = false;
			}
			if (cam.phys.targetInst != nullptr && cam.phys.targetInst != CAFE_BABE(CompInstance) && cam.phys.targetInst->isNeedingUpdated == true) {
				cam.stale = cam.stale | PKE_CAMERA_STALE_POSROT;
			}
		}
	}
}