#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 "memory.hpp"
#include "physics.hpp"

PkeCamera NullCamera {
	.camHandle = CameraHandle_MAX,
	.pos = glm::vec3(3.f, 3.f, 3.f),
	.rot = glm::quat(1.f, 0.f, 0.f, 0.f),
	.target = glm::vec3(0.f),
	.type = PKE_CAMERA_TYPE_ORTHOGONAL,
	.orientation = PKE_CAMERA_ORIENTATION_TARGET,
	.stale = PKE_CAMERA_STALE_ALL,
	.phys = {
		.inst = nullptr,
		.constraint = nullptr,
	},
};
PkeCamera *ActiveCamera = &NullCamera;

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

btSphereShape CameraShape{1.f};

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

	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 = Pke_New<btDefaultMotionState>(MemBkt_Bullet);
	new (cam.phys.inst->bt.motionState) btDefaultMotionState(instPos.posRot);

	cam.phys.inst->bt.rigidBody = Pke_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_Get(CameraHandle cameraHandle) {
	assert(cameraHandle != CameraHandle_MAX);
	return &Camera_BucketContainer.buckets[cameraHandle.bucketIndex][cameraHandle.itemIndex];
}

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

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

	btTransform trfm;
	inst->bt.motionState->getWorldTransform(trfm);
	BulletToGlm(trfm.getOrigin(), cam.target);
	trfm.setOrigin(trfm.getOrigin() + cameraOffset);
	BulletToGlm(trfm.getOrigin(), cam.pos);

	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 = Pke_New<btTypedConstraint>(MemBkt_Bullet);
	new (cam.phys.constraint) btPoint2PointConstraint(*cam.phys.inst->bt.rigidBody, *inst->bt.rigidBody, btVector3(0.f, 0.f, 0.f), cameraOffset);
	BtDynamicsWorld->addConstraint(cam.phys.constraint);

	cam.orientation = PKE_CAMERA_ORIENTATION_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);
	Pke_Delete<btTypedConstraint>(cam.phys.constraint, MemBkt_Bullet);
	cam.phys.constraint = CAFE_BABE(btTypedConstraint);
	cam.stale = PKE_CAMERA_STALE_POSROT;
}

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

	BtDynamicsWorld->removeRigidBody(cam.phys.inst->bt.rigidBody);
	Pke_Delete<btRigidBody>(cam.phys.inst->bt.rigidBody, MemBkt_Bullet);
	cam.phys.inst->bt.rigidBody = CAFE_BABE(btRigidBody);

	Pke_Delete<btDefaultMotionState>(cam.phys.inst->bt.motionState, MemBkt_Bullet);
	cam.phys.inst->bt.motionState = CAFE_BABE(btDefaultMotionState);

	if (cam.phys.constraint != nullptr && cam.phys.constraint != CAFE_BABE(btTypedConstraint)) {
		BtDynamicsWorld->removeConstraint(cam.phys.constraint);
		Pke_Delete<btTypedConstraint>(cam.phys.constraint, MemBkt_Bullet);
	}

	ECS_MarkForRemoval(&cam);
	cam.camHandle = CameraHandle_MAX;
	cam.pos = glm::vec3(0);
	cam.rot = glm::quat{};
	cam.target = glm::vec3(0);
	cam.type = PkeCameraType_MAX;
	cam.orientation = PkeCameraOrientation_MAX;
	cam.stale = PkeCameraStaleFlags_MAX;
	cam.phys.inst = 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);
}

void PkeCamera_Teardown() {
	Buckets_Destroy(Camera_BucketContainer);
}

void PkeCamera_Tick(double delta) {
	/*
	for (PkeHandleBucketIndex_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 (PkeHandleItemIndex_T i = 0; i < itemCount; ++i) {
			auto &cam = bkt[i];
			if (cam.handle == EntityHandle_MAX) {
				continue;
			}
			btTransform trfm;
			if (cam.phys.constraint != nullptr && cam.phys.constraint != CAFE_BABE(btTypedConstraint)) {
				cam.phys.constraint->getRigidBodyB().getMotionState()->getWorldTransform(trfm);
				BulletToGlm(trfm.getOrigin(), cam.target);
			}
			cam.phys.inst->bt.motionState->getWorldTransform(trfm);
			BulletToGlm(trfm.getOrigin(), cam.pos);
			cam.stale = cam.stale | PKE_CAMERA_STALE_POSROT;
			cam.phys.inst->isNeedingUpdated = false;
		}
	}
	*/
	btTransform trfm;
	if (ActiveCamera->phys.constraint != nullptr && ActiveCamera->phys.constraint != CAFE_BABE(btTypedConstraint)) {
		ActiveCamera->phys.constraint->getRigidBodyB().getMotionState()->getWorldTransform(trfm);
		BulletToGlm(trfm.getOrigin(), ActiveCamera->target);
		ActiveCamera->stale = ActiveCamera->stale | PKE_CAMERA_STALE_ROT;
	}
	if (ActiveCamera->phys.inst != nullptr && ActiveCamera->phys.inst != CAFE_BABE(CompInstance)) {
		ActiveCamera->phys.inst->bt.motionState->getWorldTransform(trfm);
		BulletToGlm(trfm.getOrigin(), ActiveCamera->pos);
		BulletToGlm(trfm.getRotation(), ActiveCamera->rot);
		ActiveCamera->stale = ActiveCamera->stale | PKE_CAMERA_STALE_POSROT;
		ActiveCamera->phys.inst->isNeedingUpdated = false;
	}
}