#include "ent.h"
#include "res.h"
#include "app.h"
#include "editor.h"

typedef struct {
	u32 gen, dense_id, next_free;
} EntitySlot;

typedef struct {
	Entity *entity;
	EntitySlot *slot;
	u32 *dense_to_slot;
	u64 *delete_queue;
	u64 *pid_to_id;

	u64 next_pid;

	/* caps are count not byte size */
	u32 entity_cnt, entity_cap;
	u32 slot_cnt, slot_cap;
	u32 delete_queue_cnt, delete_queue_cap;
	u32 pid_to_id_cap;
	u32 free_slot;
} EntitySystem;

static EntitySystem en = {0};

static u64
entity_id_create(u32 slot, u32 gen) {
	return ((u64)gen << 32) | (u64)slot;
}

static u32
entity_slot(u64 id) {
	return (u32)id;
}

static u32
entity_gen(u64 id) {
	return (u32)(id >> 32);
}

static void
check_entity_cap(u32 req) {
	if (req <= en.entity_cap) {
		return;
	}

	while (en.entity_cap < req) {
		en.entity_cap *= 2;
	}

	u32 *tmp_dts = realloc(en.dense_to_slot, en.entity_cap * sizeof(u32));
	Entity *tmp_ent = realloc(en.entity, en.entity_cap * sizeof(Entity));
	if (!tmp_dts || !tmp_ent) {
		perror("err: entity array realloc failed.");
		exit(1);
	}

	en.dense_to_slot = tmp_dts;
	en.entity = tmp_ent;
}

static void
check_slot_cap(u32 req) {
	if (req <= en.slot_cap) {
		return;
	}

	while (en.slot_cap < req) {
		en.slot_cap *= 2;
	}

	EntitySlot *tmp = realloc(en.slot, en.slot_cap * sizeof(EntitySlot));
	if (!tmp) {
		perror("err: entity slot array realloc failed.");
		exit(1);
	}

	en.slot = tmp;
}

static void
check_delete_queue_cap(u32 req) {
	if (req <= en.delete_queue_cap) {
		return;
	}

	while (en.delete_queue_cap < req) {
		en.delete_queue_cap *= 2;
	}

	u64 *tmp = realloc(en.delete_queue, en.delete_queue_cap * sizeof(u64));
	if (!tmp) {
		perror("err: failed to realloc delete queue.");
		exit(1);
	}

	en.delete_queue = tmp;
}

static void
check_pid_to_id_cap(u64 pid) {
	if (pid < en.pid_to_id_cap) {
		return;
	}

	u32 limit = UINT32_MAX / sizeof(u64);
	if (pid >= limit) {
		TraceLog(LOG_FATAL, "pid exceeds addressable table size");
	}

	u32 start_cap = en.pid_to_id_cap;

	if (!en.pid_to_id_cap) {
		en.pid_to_id_cap = 256;
	}

	while (en.pid_to_id_cap <= pid) {
		en.pid_to_id_cap *= 2;
	}

	u64 *items = realloc(en.pid_to_id, en.pid_to_id_cap * sizeof(u64));
	if (!items) {
		perror("entity PID table alloc failed.");
		exit(1);
	}
	en.pid_to_id = items;
	memset(items + start_cap, 0,
		(en.pid_to_id_cap - start_cap) * sizeof(u64));
}


static EntitySlot*
entity_get_slot(u64 id) {
	EntitySlot *slot;
	u32 slot_id;
	u32 gen;

	if (id == 0) {
		return NULL;
	}

	slot_id = entity_slot(id);
	gen = entity_gen(id);

	if (slot_id >= en.slot_cnt) {
		return NULL;
	}

	slot = &en.slot[slot_id];

	if (slot->dense_id == ENTITY_INVALID_IDX ||
		slot->gen != gen ||
		slot->dense_id >= en.entity_cnt) {
		return NULL;
	}

	return slot;
}

static void
entity_clear_hierarchy(u64 id) {
	Entity *ent = entity_get(id);
	if (!ent) {
		return;
	}

	entity_set_parent(id, 0);

	while (ent->tr.first_child) {
		entity_set_parent(ent->tr.first_child, 0);
	}
}

static void
entity_delete_now(u64 id) {
	if (!entity_valid(id)) {
		return;
	}

	u64 child;
	while ((child = entity_get(id)->tr.first_child) != 0) {
		entity_delete_now(child);
	}

	Entity extracted = entity_extract_now(id);
	entity_clean(&extracted);
}


static void
entity_process_delete_queue(void) {
	for (int i = 0; i < en.delete_queue_cnt; ++i) {
		u64 id = en.delete_queue[i];

		if (entity_valid(id)) {
			entity_delete_now(id);
		}
	}
	en.delete_queue_cnt = 0;
}

void
entity_system_init(void) {
	en.entity_cap = 256;
	en.slot_cap = 256;
	en.delete_queue_cap = 32;
	en.entity = calloc(en.entity_cap, sizeof(Entity));
	en.slot = calloc(en.slot_cap, sizeof(EntitySlot));
	en.dense_to_slot = calloc(en.entity_cap, sizeof(u32));
	en.delete_queue = calloc(en.delete_queue_cap, sizeof(u64));
	en.free_slot = ENTITY_INVALID_IDX;
	en.next_pid = 1;
}

void
entity_system_exit(void) {
	for (u32 i = 0; i < en.entity_cnt; ++i) {
		entity_clean(&en.entity[i]);
	}
	free(en.entity);
	free(en.dense_to_slot);
	free(en.slot);
	free(en.delete_queue);
	free(en.pid_to_id);
	en = (EntitySystem) {0};
}

void
entity_system_clear(void) {
	for (u32 i = 0; i < en.entity_cnt; ++i) {
		entity_clean(&en.entity[i]);
	}

	memset(en.entity, 0, en.entity_cap * sizeof(Entity));
	memset(en.slot, 0, en.slot_cap * sizeof(EntitySlot));
	memset(en.dense_to_slot, 0, en.entity_cap * sizeof(u32));
	memset(en.delete_queue, 0, en.delete_queue_cap * sizeof(u64));
	memset(en.pid_to_id, 0, en.pid_to_id_cap * sizeof(u64));

	en.entity_cnt = 0;
	en.slot_cnt = 0;
	en.delete_queue_cnt = 0;
	en.free_slot = ENTITY_INVALID_IDX;
	en.next_pid = 1;
}

static void
entity_draw_mdl(u64 id) {
	Entity *ent = entity_get(id);
	ModelCmp *model = &ent->mdl;

	if (!model->mdl) {
		Asset *asset = asset_get(model->key);
		if (!asset) {
			return;
		}
		model->mdl = &asset->mdl;
	}

	Model mdl = *model->mdl;
	mdl.transform = entity_world_matrix(id);

	Color tint = ent->ed.selected && ed.mode == ED_MODE_SCENE ?
		(Color){100, 100, 100, 255} : BLACK;

	u32 count = MIN(model->mat_cnt, (u32)mdl.materialCount);

	if (!count) {
		DrawModel(mdl, (Vec3){0}, 1, tint);
		return;
	}

	struct {
		MaterialMap albedo;
		Shader shader;
	} saved[count];

	for (u32 i = 0; i < count; ++i) {
		Material *material = &mdl.materials[i];
		MaterialMap *albedo;
		albedo = &material->maps[MATERIAL_MAP_ALBEDO];

		saved[i].albedo = *albedo;
		saved[i].shader = material->shader;

		MaterialOverride *override = &model->materials[i];

		if (override->tex_key) {
			Asset *asset = asset_get(override->tex_key);
			if (asset) {
				albedo->texture = asset->tex;
			}
		}

		if (override->shader_key) {
			FunShader *shader;
			shader = shader_get(override->shader_key);

			if (shader) {
				material->shader = shader->sdr;
			}
		}
	}

	DrawModel(mdl, (Vec3){0}, 1, tint);

	for (u32 i = 0; i < count; ++i) {
		Material *material = &mdl.materials[i];
		material->maps[MATERIAL_MAP_ALBEDO] = saved[i].albedo;
		material->shader = saved[i].shader;
	}
}

void
entity_system_update(void) {
	entity_process_delete_queue();

	if (physics.simulate) {
		return;
	}

	for (u32 i = 0; i < entity_count(); ++i) {
		u64 id = entity_id_at(i);
		Entity *ent = entity_get(id);
		PhysicsCmp *phys = &ent->phys;

		if (phys->has && phys->body_cnt && !phys->runtime.bodies) {
			entity_physics_build(id);
		}
	}
}

void
entity_system_draw_3d(void) {
	for (u32 i = 0; i < entity_count(); ++i) {
		u64 id = entity_id_at(i);
		Entity *ent = entity_get(id);

		if (app.mode == APP_MODE_EDIT &&
			ed.mode == ED_MODE_PHYSICS &&
			ent->pid != ed.physics_pid) {
			continue;
		}

		if (ent->mdl.has && ent->mdl.draw) {
			entity_draw_mdl(id);
		}
	}
}

void
entity_system_set_next_pid(u64 pid) {
	en.next_pid = pid;
}

void
entities_clear_selected(void) {
	for (int i = 0; i < en.entity_cnt; ++i) {
		Entity *ent = entity_get(entity_id_at(i));
		ent->ed.selected = false;
	}
}

u64
entity_create(bool transient) {
	check_entity_cap(en.entity_cnt + 1);

	u32 slot_id = 0;
	EntitySlot *slot;

	if (en.free_slot != ENTITY_INVALID_IDX) {
		slot_id = en.free_slot;
		slot = &en.slot[slot_id];
		en.free_slot = slot->next_free;
	} else {
		check_slot_cap(en.slot_cnt + 1);
		slot_id = en.slot_cnt;
		en.slot_cnt += 1;
		slot = &en.slot[slot_id];
		slot->gen = 1;
	}

	u32 dense_id = en.entity_cnt;
	en.entity_cnt += 1;

	slot->dense_id = dense_id;
	slot->next_free = ENTITY_INVALID_IDX;
	en.dense_to_slot[dense_id] = slot_id;

	en.entity[dense_id] = (Entity){0};
	en.entity[dense_id].tr.rot = QuaternionIdentity();
	en.entity[dense_id].tr.scale = Vector3One();

	u64 id = entity_id_create(slot_id, slot->gen);
  	if (!transient) {
  		u64 pid = en.next_pid++;
  		check_pid_to_id_cap(pid);
  		en.entity[dense_id].name = strdup("entity");
  		en.entity[dense_id].pid = pid;
  		en.pid_to_id[pid] = id;
  	}
	return id;
}

u64
entity_restore(u64 pid) {
	if (!pid || entity_id_from_pid(pid)) {
		return 0;
	}

	check_pid_to_id_cap(pid);

	u64 id = entity_create(true);
	Entity *ent = entity_get(id);
	ent->pid = pid;
	en.pid_to_id[pid] = id;
	en.next_pid = MAX(en.next_pid, pid + 1);

	return id;
}

/* deferred delete */
void
entity_delete(u64 id) {
	if (!entity_valid(id)) {
		return;
	}

	check_delete_queue_cap(en.delete_queue_cnt + 1);
	en.delete_queue[en.delete_queue_cnt] = id;
	en.delete_queue_cnt += 1;
}

ModelCmp
entity_model_copy(const ModelCmp *src) {
	ModelCmp copy = *src;
	copy.key = src->key ? strdup(src->key) : NULL;
	copy.materials = NULL;

	if (src->mat_cnt) {
		copy.materials = calloc(src->mat_cnt,
		                        sizeof(*copy.materials));
		if (!copy.materials) {
			perror("entity_model_copy: alloc failed.");
			exit(1);
		}

		for (u32 i = 0; i < src->mat_cnt; ++i) {
			const MaterialOverride *from = &src->materials[i];
			MaterialOverride *to = &copy.materials[i];

			to->tex_key = from->tex_key ?
				strdup(from->tex_key) : NULL;
			to->shader_key = from->shader_key ?
				strdup(from->shader_key) : NULL;
		}
	}

	return copy;
}

void
entity_model_free(ModelCmp *model) {
	for (u32 i = 0; i < model->mat_cnt; ++i) {
		free(model->materials[i].tex_key);
		free(model->materials[i].shader_key);
	}

	free(model->materials);
	free(model->key);
	*model = (ModelCmp){0};
}

static void *
physics_copy_data(const void *src, size_t size) {
	if (!size) {
		return NULL;
	}

	void *copy = malloc(size);
	if (!copy) {
		perror("physics_copy_data: alloc failed");
		exit(1);
	}

	memcpy(copy, src, size);
	return copy;
}

PhysicsCmp
entity_physics_copy(const PhysicsCmp *src) {
	PhysicsCmp copy = *src;

	copy.runtime.bodies = NULL;
	copy.runtime.joints = NULL;

	copy.bodies = physics_copy_data(
		src->bodies, src->body_cnt * sizeof(*src->bodies));
	copy.joints = physics_copy_data(
		src->joints, src->joint_cnt * sizeof(*src->joints));

	for (u32 i = 0; i < src->body_cnt; ++i) {
		const PhysicsBody *source = &src->bodies[i];
		PhysicsBody *body = &copy.bodies[i];

		body->shapes = physics_copy_data(
			source->shapes,
			source->shape_cnt * sizeof(*source->shapes));

		for (u32 j = 0; j < body->shape_cnt; ++j) {
			PhysicsShape *shape = &body->shapes[j];

			switch (shape->tag) {
			case PHYS_SHAPE_CONVEX_HULL:
				shape->ConvexHull.points = physics_copy_data(
					shape->ConvexHull.points,
					shape->ConvexHull.point_cnt *
					sizeof(*shape->ConvexHull.points));
				break;

			case PHYS_SHAPE_STATIC_TRIMESH:
				shape->TriMesh.points = physics_copy_data(
					shape->TriMesh.points,
					shape->TriMesh.point_cnt *
					sizeof(*shape->TriMesh.points));
				shape->TriMesh.indices = physics_copy_data(
					shape->TriMesh.indices,
					shape->TriMesh.idx_cnt *
					sizeof(*shape->TriMesh.indices));
				break;

			default:
				break;
			}
		}
	}

	return copy;
}

void
entity_physics_clear(PhysicsCmp *phys) {
	if (phys->runtime.joints) {
		for (int i = 0; i < phys->joint_cnt; ++i) {
			b3JointId joint_id = phys->runtime.joints[i];
			if (b3Joint_IsValid(joint_id)) {
				b3DestroyJoint(joint_id, false);
			}
		}
	}

	if (phys->runtime.bodies) {
		for (int i = 0; i < phys->body_cnt; ++i) {
			PhysicsBodyRuntime *bdy = &phys->runtime.bodies[i];
			if (b3Body_IsValid(bdy->id)) {
				b3DestroyBody(bdy->id);
			}
			free(bdy->shapes);
		}
	}

	free(phys->runtime.joints);
	free(phys->runtime.bodies);
	phys->runtime.joints = NULL;
	phys->runtime.bodies = NULL;
}

static void
physics_build_shapes(u64 id, u32 body_index) {
	Entity *ent = entity_get(id);
	PhysicsBody *body = &ent->phys.bodies[body_index];
	PhysicsBodyRuntime *runtime = &ent->phys.runtime.bodies[body_index];

	if (!body->shape_cnt) {
		return;
	}

	runtime->shapes = calloc(
		body->shape_cnt, sizeof(*runtime->shapes));

	if (!runtime->shapes) {
		perror("physics shape handles allocation failed");
		exit(EXIT_FAILURE);
	}

	Matrix entity_world = entity_world_matrix(id);
	Quaternion entity_rot = entity_world_rot(id);

	Vec3 pos = Vector3Transform(body->pos, entity_world);
	Quaternion rot = QuaternionMultiply(entity_rot, body->rot);
	Quaternion inverse_rot = QuaternionInvert(rot);

	Matrix body_local = MatrixMultiply(
		QuaternionToMatrix(body->rot),
		MatrixTranslate(body->pos.x, body->pos.y, body->pos.z));

	Matrix body_world = MatrixMultiply(body_local, entity_world);

	for (u32 j = 0; j < body->shape_cnt; ++j) {
		PhysicsShape *shape = &body->shapes[j];

		Matrix shape_local = MatrixMultiply(
			QuaternionToMatrix(shape->rot),
			MatrixTranslate(
				shape->pos.x, shape->pos.y, shape->pos.z));

		Matrix world = MatrixMultiply(shape_local, body_world);

		Vec3 scale = {
			Vector3Length((Vec3){world.m0, world.m1, world.m2}),
			Vector3Length((Vec3){world.m4, world.m5, world.m6}),
			Vector3Length((Vec3){world.m8, world.m9, world.m10}),
		};

		Vec3 center = {world.m12, world.m13, world.m14};
		center = Vector3RotateByQuaternion(
			Vector3Subtract(center, pos), inverse_rot);

		b3ShapeDef def = b3DefaultShapeDef();
		def.density = shape->density;
		def.baseMaterial.friction = shape->friction;
		def.baseMaterial.restitution = shape->restitution;

		def.filter.categoryBits = shape->category_bits |
			BIT(PHYS_MASK_EDITOR_SELECTABLE);
		def.filter.maskBits = shape->mask_bits |
			BIT(PHYS_MASK_EDITOR_RAYCAST);

		switch (shape->tag) {
		case PHYS_SHAPE_BOX: {
			Vec3 size = Vector3Multiply(shape->Box.sz, scale);

			if (size.x <= 0 || size.y <= 0 || size.z <= 0) {
				break;
			}

			b3Transform local = {
				.p = {center.x, center.y, center.z},
				.q = {
					.v = {
						shape->rot.x,
						shape->rot.y,
						shape->rot.z,
					},
					.s = shape->rot.w,
				},
			};

			b3BoxHull hull = b3MakeTransformedBoxHull(
				size.x * 0.5f,
				size.y * 0.5f,
				size.z * 0.5f,
				local);

			runtime->shapes[j] = b3CreateHullShape(
				runtime->id, &def, &hull.base);
			break;
		}

		case PHYS_SHAPE_SPHERE: {
			Vec3 entity_scale = {
				Vector3Length((Vec3){
					entity_world.m0,
					entity_world.m1,
					entity_world.m2,
				}),
				Vector3Length((Vec3){
					entity_world.m4,
					entity_world.m5,
					entity_world.m6,
				}),
				Vector3Length((Vec3){
					entity_world.m8,
					entity_world.m9,
					entity_world.m10,
				}),
			};

			f32 largest = fmaxf(entity_scale.x,
				fmaxf(entity_scale.y, entity_scale.z));
			f32 radius = shape->Sphere.radius * largest;

			if (radius <= 0) {
				break;
			}

			b3Sphere sphere = {
				.center = {center.x, center.y, center.z},
				.radius = radius,
			};

			runtime->shapes[j] = b3CreateSphereShape(
				runtime->id, &def, &sphere);
			break;
		}

		case PHYS_SHAPE_CAPSULE: {
			f32 radius = shape->Capsule.radius *
				fmaxf(scale.x, scale.z);
			f32 half_height =
				shape->Capsule.height * scale.y * 0.5f;

			if (radius <= 0) {
				break;
			}

			if (half_height == 0) {
				b3Sphere sphere = {
					.center = {
						center.x,
						center.y,
						center.z
					},
					.radius = radius,
				};

				runtime->shapes[j] = b3CreateSphereShape(
					runtime->id, &def, &sphere);
				break;
			}

			Vec3 offset = Vector3RotateByQuaternion(
				(Vec3){0, half_height, 0}, shape->rot);

			Vec3 start = Vector3Subtract(center, offset);
			Vec3 end = Vector3Add(center, offset);

			b3Capsule capsule = {
				.center1 = {start.x, start.y, start.z},
				.center2 = {end.x, end.y, end.z},
				.radius = radius,
			};

			runtime->shapes[j] = b3CreateCapsuleShape(
				runtime->id, &def, &capsule);
			break;
		}

		case PHYS_SHAPE_CYLINDER: {
			f32 radius = shape->Cylinder.radius *
				fmaxf(scale.x, scale.z);
			f32 height = shape->Cylinder.height * scale.y;

			if (radius <= 0 || height <= 0) {
				break;
			}

			b3HullData *hull = b3CreateCylinder(
				height, radius, -height * 0.5f, 12);

			if (!hull) {
				TraceLog(LOG_ERROR,
					"cylinder hull creation failed");
				break;
			}

			b3Transform local = {
				.p = {center.x, center.y, center.z},
				.q = {
					.v = {
						shape->rot.x,
						shape->rot.y,
						shape->rot.z,
					},
					.s = shape->rot.w,
				},
			};

			runtime->shapes[j] = b3CreateTransformedHullShape(
				runtime->id, &def, hull, local,
				(b3Vec3){1, 1, 1});

			b3DestroyHull(hull);
			break;
		}

		default:
			break;
		}
	}
}

void
entity_physics_build(u64 id) {
	Entity *ent = entity_get(id);
	if (!ent) {
		TraceLog(LOG_ERROR,
			"entity_physics_build: invalid entity id");
		return;
	}

	PhysicsCmp *phys = &ent->phys;
	entity_physics_clear(phys);

	if (!phys->has || !phys->body_cnt) {
		return;
	}

	phys->runtime.bodies = calloc(phys->body_cnt,
	                        sizeof(*phys->runtime.bodies));

	if (!phys->runtime.bodies) {
		perror("entity_physics_build: allocation failed");
		exit(1);
	}

	Matrix entity_world = entity_world_matrix(id);
	Quaternion entity_rot = entity_world_rot(id);

	phys->runtime.world = entity_world;
	phys->runtime.world_rot = entity_rot;

	for (u32 i = 0; i < phys->body_cnt; ++i) {
		PhysicsBody *body = &phys->bodies[i];
		PhysicsBodyRuntime *runtime = &phys->runtime.bodies[i];

		runtime->entity_id = id;

		Vec3 pos = Vector3Transform(body->pos, entity_world);
		Quaternion rot = QuaternionMultiply(entity_rot, body->rot);

		b3BodyDef def = b3DefaultBodyDef();

		switch (body->mode) {
		case PHYS_MODE_STATIC:
			def.type = b3_staticBody;
			break;

		case PHYS_MODE_KINEMATIC:
			def.type = b3_kinematicBody;
			break;

		case PHYS_MODE_DYNAMIC:
			def.type = b3_dynamicBody;
			break;
		}

		def.position = (b3Pos){pos.x, pos.y, pos.z};
		def.rotation = (b3Quat){
			.v = {rot.x, rot.y, rot.z},
			.s = rot.w,
		};

		def.linearDamping = body->linear_damping;
		def.angularDamping = body->angular_damping;
		def.gravityScale = body->gravity_scale;
		def.enableSleep = body->enable_sleep;
		def.userData = &runtime->entity_id;

		runtime->id = b3CreateBody(physics.world_id, &def);
		physics_build_shapes(id, i);
	}
}

void
entity_physics_free(PhysicsCmp *phys) {
	entity_physics_clear(phys);

	for (u32 i = 0; i < phys->body_cnt; ++i) {
		PhysicsBody *body = &phys->bodies[i];

		for (u32 j = 0; j < body->shape_cnt; ++j) {
			PhysicsShape *shape = &body->shapes[j];

			switch (shape->tag) {
			case PHYS_SHAPE_CONVEX_HULL:
				free(shape->ConvexHull.points);
				break;

			case PHYS_SHAPE_STATIC_TRIMESH:
				free(shape->TriMesh.points);
				free(shape->TriMesh.indices);
				break;

			default:
				break;
			}
		}

		free(body->shapes);
	}

	free(phys->bodies);
	free(phys->joints);
	*phys = (PhysicsCmp){0};
}

void
entity_physics_transform_changed(u64 id) {
	if (physics.simulate) {
		return;
	}

	Entity *ent = entity_get(id);
	assert(ent);

	/* Children inherit this entity's transform. */
	for (u64 child = ent->tr.first_child; child;
		child = entity_get(child)->tr.next) {
		entity_physics_transform_changed(child);
	}

	PhysicsCmp *phys = &ent->phys;
	if (!phys->runtime.bodies) {
		return;
	}

	Matrix world = entity_world_matrix(id);
	Quaternion world_rot = entity_world_rot(id);

	/* Remove world rotation, leaving scale and distortion. */
	Matrix previous = MatrixMultiply(
		phys->runtime.world,
		QuaternionToMatrix(
			QuaternionInvert(phys->runtime.world_rot)));

	Matrix current = MatrixMultiply(
		world,
		QuaternionToMatrix(QuaternionInvert(world_rot)));

	float16 before = MatrixToFloatV(previous);
	float16 after = MatrixToFloatV(current);

	/* Compare only the upper-left 3x3; translation is irrelevant. */
	for (int column = 0; column < 3; ++column) {
		for (int row = 0; row < 3; ++row) {
			int index = column * 4 + row;

			if (!FloatEquals(before.v[index], after.v[index])) {
				entity_physics_clear(phys);
				return;
			}
		}
	}

	/* Geometry still fits: update the existing body poses. */
	for (u32 i = 0; i < phys->body_cnt; ++i) {
		PhysicsBody *body = &phys->bodies[i];

		Vec3 pos = Vector3Transform(body->pos, world);
		Quaternion rot = QuaternionMultiply(world_rot, body->rot);

		b3Body_SetTransform(
			phys->runtime.bodies[i].id,
			(b3Pos){pos.x, pos.y, pos.z},
			(b3Quat){
				.v = {rot.x, rot.y, rot.z},
				.s = rot.w,
			});
	}
}

void
entity_clean(Entity *ent) {
	free(ent->name);

	/* model component data */
	entity_model_free(&ent->mdl);

	/* physics component data */
	entity_physics_free(&ent->phys);
}

Entity
entity_extract_now(u64 id) {
	EntitySlot *slot = entity_get_slot(id);
	assert(slot);

	u32 slot_id = entity_slot(id);
	u32 dense_id = slot->dense_id;
	u32 last_dense_id = en.entity_cnt - 1;

	assert(!en.entity[dense_id].tr.first_child);

	entity_physics_clear(&en.entity[dense_id].phys);
	entity_set_parent(id, 0);
	Entity extracted = en.entity[dense_id];

	if (extracted.pid) {
		en.pid_to_id[extracted.pid] = 0;
	}

	if (dense_id != last_dense_id) {
		u32 moved_slot_id = en.dense_to_slot[last_dense_id];
		en.entity[dense_id] = en.entity[last_dense_id];
		en.dense_to_slot[dense_id] = moved_slot_id;
		en.slot[moved_slot_id].dense_id = dense_id;
	}

	en.entity[last_dense_id] = (Entity){0};
	en.dense_to_slot[last_dense_id] = ENTITY_INVALID_IDX;
	en.entity_cnt--;

	slot->dense_id = ENTITY_INVALID_IDX;
	slot->gen++;

	if (slot->gen == 0) {
		slot->gen = 1;
	}

	slot->next_free = en.free_slot;
	en.free_slot = slot_id;

	return extracted;
}

static void
entity_unlink_parent(u64 id) {
	Entity *ent = entity_get(id);
	if (!ent) {
		return;
	}

	TransformCmp *tr = &ent->tr;

	if (tr->prev) {
		Entity *prev = entity_get(tr->prev);
		if (prev) {
			prev->tr.next = tr->next;
		}
	} else if (tr->parent) {
		Entity *parent = entity_get(tr->parent);
		if (parent && parent->tr.first_child == id) {
			parent->tr.first_child = tr->next;
		}
	}

	if (tr->next) {
		Entity *next = entity_get(tr->next);
		if (next) {
			next->tr.prev = tr->prev;
		}
	}

	tr->parent = 0;
	tr->prev = 0;
	tr->next = 0;
}

static bool
entity_is_ancestor(u64 ancestor_id, u64 id) {
	while (id) {
		if (id == ancestor_id) {
			return true;
		}

		Entity *ent = entity_get(id);
		if (!ent) {
			return false;
		}

		id = ent->tr.parent;
	}

	return false;
}

bool
entity_set_parent(u64 id, u64 parent_id) {
	Entity *ent = entity_get(id);
	if (!ent) {
		TraceLog(LOG_WARNING, "entity_set_parent: entity id null");
		return false;
	}

	if (parent_id == ent->tr.parent) {
		return true;
	}

	Entity *parent = NULL;
	if (parent_id) {
		parent = entity_get(parent_id);
		if (!parent ||
			parent_id == id ||
			entity_is_ancestor(id, parent_id)) {
			TraceLog(LOG_ERROR, "entity hierarchy failed.");
			return false;
		}
	}

	entity_unlink_parent(id);

	if (!parent) {
		entity_physics_transform_changed(id);
		return true;
	}

	u64 first_child = parent->tr.first_child;
	ent->tr.parent = parent_id;
	ent->tr.next = first_child;

	if (first_child) {
		Entity *first = entity_get(first_child);
		if (first) {
			first->tr.prev = id;
		}
	}

	parent->tr.first_child = id;
	entity_physics_transform_changed(id);
	return true;
}

bool
entity_valid(u64 id) {
	return entity_get_slot(id) != NULL;
}

Entity*
entity_get(u64 id) {
	EntitySlot *slot = entity_get_slot(id);
	if (!slot) {
		return NULL;
	}

	return &en.entity[slot->dense_id];
}

u64
entity_id_at(u32 dense_id) {
	if (dense_id >= en.entity_cnt) {
		return 0;
	}

	u32 slot_id = en.dense_to_slot[dense_id];
	EntitySlot *slot = &en.slot[slot_id];

	return entity_id_create(slot_id, slot->gen);
}

u64
entity_id_from_pid(u64 pid) {
	return pid < en.pid_to_id_cap ? en.pid_to_id[pid] : 0;
}

Entity*
entity_data(void) {
	return en.entity;
}

u32
entity_count(void) {
	return en.entity_cnt;
}

u64
entity_next_pid(void) {
	return en.next_pid;
}

/* entity transform section */
Matrix
entity_world_matrix(u64 id) {
	Matrix world = MatrixIdentity();

	while (id) {
		Entity *ent = entity_get(id);
		assert(ent);

		TransformCmp *tr = &ent->tr;

		Matrix scale = MatrixScale(
			tr->scale.x, tr->scale.y, tr->scale.z);
		Matrix rotation = QuaternionToMatrix(tr->rot);
		Matrix translation = MatrixTranslate(
			tr->pos.x, tr->pos.y, tr->pos.z);

		/* Raymath order: apply scale, rotation, translation. */
		Matrix local = MatrixMultiply(
			MatrixMultiply(scale, rotation), translation);

		world = MatrixMultiply(world, local);
		id = tr->parent;
	}

	return world;
}

Vec3
entity_point_to_world(u64 space, Vec3 point) {
	return Vector3Transform(point, entity_world_matrix(space));
}

bool
entity_point_to_local(u64 space, Vec3 point, Vec3 *out) {
	if (space) {
		Entity *ent = entity_get(space);
		assert(ent);

		TransformCmp *tr = &ent->tr;
		if (!tr->scale.x || !tr->scale.y || !tr->scale.z) {
			return false;
		}

		if (!entity_point_to_local(tr->parent, point, &point)) {
			return false;
		}
		point = Vector3Subtract(point, tr->pos);
		point = Vector3RotateByQuaternion(point,
		                        	QuaternionInvert(tr->rot));
		point = Vector3Divide(point, tr->scale);
	}

	*out = point;
	return true;
}

Vec3
entity_world_pos(u64 id) {
	return entity_point_to_world(id, Vector3Zero());
}

Quaternion
entity_world_rot(u64 id) {
	Quaternion rot = QuaternionIdentity();

	while (id) {
		Entity *ent = entity_get(id);
		assert(ent);

		rot = QuaternionMultiply(ent->tr.rot, rot);
		id = ent->tr.parent;
	}

	return QuaternionNormalize(rot);
}

void
entity_set_world_pos(Entity *ent, Vec3 pos) {
	if (!entity_point_to_local(ent->tr.parent, pos, &ent->tr.pos)) {
		TraceLog(LOG_WARNING, "Cannot set world position through a "
			"zero-scale ancestor.");
	}
}

void
entity_set_world_rot(Entity *ent, Quaternion rot) {
	f32 qlen = QuaternionLength(rot);
	if (!ent || !qlen || !isfinite(qlen)) {
		return;
	}

	ent->tr.rot =
		QuaternionNormalize(QuaternionMultiply(
	                QuaternionInvert(entity_world_rot(ent->tr.parent)),
	        		QuaternionNormalize(rot)));
}