#include "animation.h"

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

static vec3_t lerp(vec3_t v, vec3_t u, double t)
{
	return vec3_add(vec3_scale(1 - t, v), vec3_scale(t, u));
}

static vec4_t slerp(vec4_t v, vec4_t u, double t)
{
	float dot = vec4_dot(v, u);
	if (dot < 0) {
		dot = -dot;
		u = vec4_scale(-1, u);
	}

	if (dot < 0.95) {
		const float a = acosf(dot);
		return vec4_scale(
		    1 / sin(a),
		    vec4_add(
		        vec4_scale(sinf(a * (1 - t)), v),
		        vec4_scale(sinf(a * t), u)));
	} else {
		return vec4_add(vec4_scale(1 - t, v), vec4_scale(t, u));
	}
}

static void interpolate(
    bone_t *dst, const bone_t *as, const bone_t *bs, size_t n, double t)
{
	for (unsigned i = 0; i < n; ++i) {
		dst[i].translation = lerp(as[i].translation, bs[i].translation, t);
		dst[i].rotation = slerp(as[i].rotation, bs[i].rotation, t);
	}
}

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

static void load_keyframe(animation_state_t *state, const model_t *model)
{
	const uint32_t keyframe_count = model->animation.keyframe_count;
	if (state->current_keyframe + 1 < keyframe_count) {
		state->next_keyframe = state->current_keyframe + 1;
		state->duration_us = model->animation.times_us[state->next_keyframe]
		    - model->animation.times_us[state->current_keyframe];
	} else {
		// Handle loop-around
		state->next_keyframe = 0;
		state->duration_us = model->animation.length_us
		    - model->animation.times_us[state->current_keyframe]
		    + model->animation.times_us[state->next_keyframe];
	}

	state->current = model->animation.keyframes
	    + state->current_keyframe * model->bone_count;
	state->next = model->animation.keyframes
	    + state->next_keyframe * model->bone_count;
}

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, uint32_t start_keyframe)
{
	state->elapsed_us = 0;
	state->current_keyframe = start_keyframe;
	load_keyframe(state, model);
}

void animation_update(
    animation_state_t *state, const model_t *model, uint32_t dt_us)
{
	state->elapsed_us += dt_us;
	while (state->elapsed_us >= state->duration_us) {
		assert(state->duration_us != 0);
		state->elapsed_us -= state->duration_us;
		state->current_keyframe = state->next_keyframe;
		load_keyframe(state, model);
	}

	bone_t bones[MAX_BONE_COUNT];
	interpolate(
	    bones, state->current, state->next, model->bone_count,
	    (double)state->elapsed_us / state->duration_us);

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