#include "animation.h"

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>

static mat4_t bone_transform(bone_t bone)
{
	return mat4_mul(
	    mat4_translate_vec3(bone.translation),
	    mat4_rotate_quaternion(bone.rotation));
}

static void compute_bone_matrices(
    const bone_t *bones, size_t bone_count, const uint32_t *bone_parents,
    const mat4_t *inverse_bind_matrices, mat4_t *matrices)
{
	matrices[0] = bone_transform(bones[0]);
	for (unsigned i = 1; i < bone_count; ++i) {
		const mat4_t transform = bone_transform(bones[i]);
		const uint32_t parent = bone_parents[i];
		assert(parent < bone_count);
		matrices[i] = mat4_mul(matrices[parent], transform);
	}

	for (unsigned i = 0; i < bone_count; ++i)
		matrices[i] = mat4_mul(matrices[i], inverse_bind_matrices[i]);
}

animation_state_t animation_init(const model_t *model)
{
	mat4_t *bone_matrices = malloc(model->bone_count * sizeof(mat4_t));
	assert(bone_matrices != nullptr);
	return (animation_state_t) { .bone_matrices = bone_matrices };
}

void animation_cleanup(animation_state_t *state)
{
	if (state->bone_matrices != nullptr) {
		free(state->bone_matrices);
		state->bone_matrices = nullptr;
	}
}

void animation_start(animation_state_t *state, const model_t *model)
{
	state->current = model->bones;
}

void animation_update(animation_state_t *state, const model_t *model)
{
	const size_t offset = state->keyframe * model->bone_count;
	state->current = model->animation.keyframes + offset;

	compute_bone_matrices(
	    state->current, model->bone_count, model->bone_parents,
	    model->inverse_bind_matrices, state->bone_matrices);

	++state->keyframe;
	if (state->keyframe >= model->animation.keyframe_count)
		state->keyframe = 0;
}