#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"
#include "vendor-glm-include.hpp"

PkeCamera NullCamera {};
CompInstance NullCameraInstance{};
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, CompInstance &inst, const InstPos &instPos) {
	btVector3 gravity(0.f, 0.f, 0.f);

	inst.comp_instance_flags |= COMPONENT_INSTANCE_FLAG_DO_NOT_SERIALIZE;
	cam.phys.instHandle = inst.instanceHandle;
	inst.physicsLayer = PhysicsCollision{0};
	inst.physicsMask = PhysicsCollision{0};

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

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

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

	return cam;
}

PkeCamera &PkeCamera_Register(pk_uuid uuid, const InstPos &instPos) {
	CameraHandle cameraHandle{Buckets_NewHandle(Camera_BucketContainer)};
	CompInstance *inst;

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

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

	return PkeCamera_Register_Inner(cam, *inst, 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_TargetInstance(CameraHandle cameraHandle, CompInstance *inst) {
	assert(cameraHandle != CameraHandle_MAX);
	auto &cam = Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
	CompInstance *selfInstance = ECS_GetInstance(cam.phys.instHandle);

	if (cam.phys.constraint != nullptr && cam.phys.constraint != CAFE_BABE(btTypedConstraint)) {
		PkeCamera_UntargetInstance(cameraHandle);
	}

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

	btTransform trfm;
	btQuaternion bQuatRot;
	inst->bt.motionState->getWorldTransform(trfm);

	glm::vec3 gPos;
	BulletToGlm(trfm.getOrigin(), gPos);
	if (gPos != glm::vec3(0, 0, 0)) {
		gPos = glm::normalize(-gPos);
	}

	glm::quat gQuatRot = glm::quatLookAt(-gPos, glm::vec3(0, -1, 0));
	GlmToBullet(gQuatRot, bQuatRot);

	trfm.setOrigin(trfm.getOrigin() + cameraOffset);
	trfm.setRotation(bQuatRot);

	cam.phys.target_inst_uuid = inst->uuid;
	cam.phys.target_inst_handle = inst->instanceHandle;
	selfInstance->bt.motionState->setWorldTransform(trfm);
	selfInstance->bt.rigidBody->setWorldTransform(trfm);
	selfInstance->bt.rigidBody->setLinearVelocity(btVector3(0,0,0));
	selfInstance->bt.rigidBody->setAngularVelocity(btVector3(0,0,0));
	selfInstance->bt.rigidBody->activate();

	assert(cam.phys.constraint == nullptr || cam.phys.constraint == CAFE_BABE(btTypedConstraint));
	/* 2025-01-14 JCB
	 * This was throwing bullet3 errors, so I've decided to split the work.
	 * This commit completes the 'targeting' task, where a camera tracks a target.
	 * This may or may not make a good starting point for the 'following' task.
	 *
	cam.phys.constraint = pk_new<btTypedConstraint>(MemBkt_Bullet);
	new (cam.phys.constraint) btPoint2PointConstraint(*selfInstance->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_UntargetInstance(CameraHandle cameraHandle) {
	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.instHandle = InstanceHandle_MAX;
	cam.phys.target_inst_uuid = pk_uuid_zed;
	cam.phys.constraint = CAFE_BABE(btTypedConstraint);
}

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

PkeCamera *PkeCamera_GetCameras(pk_handle_bucket_index_T bucketIndex, pk_handle_item_index_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.type = PKE_CAMERA_TYPE_ORTHOGONAL;
	NullCamera.view = PKE_CAMERA_VIEW_TARGET;
	NullCamera.stale = PKE_CAMERA_STALE_ALL;
	InstPos instPos{
		.posRot = {},
		.scale = btVector3(1.f, 1.f, 1.f),
		.mass = 1.f,
	};
	PkeCamera_Register_Inner(NullCamera, NullCameraInstance, instPos);
}

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

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.
	 */
	(void)delta;
	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 || cam.phys.instHandle == InstanceHandle_MAX) {
				continue;
			}
			CompInstance *inst = ECS_GetInstance(cam.phys.instHandle);
			assert(inst != nullptr);
			if (inst->isNeedingUpdated == true) {
				cam.stale = cam.stale | PKE_CAMERA_STALE_POSROT;
				inst->isNeedingUpdated = false;
			}
			if (cam.phys.target_inst_uuid == pk_uuid_zed) continue;
			cam.stale = cam.stale | PKE_CAMERA_STALE_POSROT;
		}
	}
}