/*
  Copyright (c) 2018 Alex Diener
  
  This software is provided 'as-is', without any express or implied
  warranty. In no event will the authors be held liable for any damages
  arising from the use of this software.
  
  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:
  
  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
  
  Alex Diener alex@ludobloom.com
*/

#include "gamemath/ArmatureAnimation.h"
#include "gamemath/BezierCurve.h"
#include "utilities/IOUtilities.h"
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#define stemobject_implementation ArmatureAnimation
stemobject_vtable_begin();
stemobject_vtable_entry(dispose);
stemobject_vtable_end();

ArmatureAnimation * ArmatureAnimation_create(bool loop, unsigned int keyframeCount, ArmatureAnimationKeyframe * keyframes, unsigned int markerCount, ArmatureAnimationMarker * markers) {
	stemobject_create_implementation(init, loop, keyframeCount, keyframes, markerCount, markers)
}

bool ArmatureAnimation_init(ArmatureAnimation * self, bool loop, unsigned int keyframeCount, ArmatureAnimationKeyframe * keyframes, unsigned int markerCount, ArmatureAnimationMarker * markers) {
	call_super(init, self);
	self->loop = loop;
	assert(keyframeCount > 0);
	self->keyframeCount = keyframeCount;
	self->keyframes = malloc(sizeof(*self->keyframes) * keyframeCount);
	for (unsigned int keyframeIndex = 0; keyframeIndex < keyframeCount; keyframeIndex++) {
		self->keyframes[keyframeIndex].interval = keyframes[keyframeIndex].interval;
		self->keyframes[keyframeIndex].boneCount = keyframes[keyframeIndex].boneCount;
		self->keyframes[keyframeIndex].bones = memdup(keyframes[keyframeIndex].bones, sizeof(*self->keyframes[keyframeIndex].bones) * keyframes[keyframeIndex].boneCount);
	}
	self->markerCount = markerCount;
	if (self->markerCount == 0) {
		self->markers = NULL;
	} else {
		self->markers = malloc(sizeof(*self->markers) * markerCount);
		memcpy(self->markers, markers, sizeof(*self->markers) * markerCount);
	}
	return true;
}

void ArmatureAnimation_dispose(ArmatureAnimation * self) {
	for (unsigned int keyframeIndex = 0; keyframeIndex < self->keyframeCount; keyframeIndex++) {
		free(self->keyframes[keyframeIndex].bones);
	}
	free(self->keyframes);
	free(self->markers);
	call_super(dispose, self);
}

double ArmatureAnimation_getLength(ArmatureAnimation * self) {
	double length = 0.0;
	for (unsigned int keyframeIndex = 0; keyframeIndex < self->keyframeCount; keyframeIndex++) {
		length += self->keyframes[keyframeIndex].interval;
	}
	return length;
}

ArmatureAnimationMarker * ArmatureAnimation_getMarkerAtTime(ArmatureAnimation * self, double time) {
	ArmatureAnimationMarker * bestMarker = NULL;
	if (self->loop) {
		double animationLength = ArmatureAnimation_getLength(self);
		time = fmod(time, animationLength);
		if (time < 0.0) {
			time += animationLength;
		}
	}
	for (unsigned int markerIndex = 0; markerIndex < self->markerCount; markerIndex++) {
		if (self->markers[markerIndex].time <= time) {
			if (bestMarker == NULL || bestMarker->time < self->markers[markerIndex].time) {
				bestMarker = &self->markers[markerIndex];
			}
		} else if (self->loop) {
			if (bestMarker == NULL || (bestMarker->time > time && self->markers[markerIndex].time > bestMarker->time)) {
				bestMarker = &self->markers[markerIndex];
			}
		}
	}
	return bestMarker;
}

static bool findBoneKeyframes(ArmatureAnimation * self, ArmatureBoneID boneID, double animationTime, double animationLength, unsigned int * outKeyframeIndexLeft, unsigned int * outKeyframeBoneIndexLeft, unsigned int * outKeyframeIndexRight, unsigned int * outKeyframeBoneIndexRight, float * outKeyframeWeight) {
	double keyframeTime = 0.0, keyframeTimeLeft = 0.0, keyframeTimeRight = 0.0;
	unsigned int keyframeIndex, keyframeIndexLeft = 0, keyframeIndexRight = 0;
	unsigned int keyframeBoneIndexLeft = BONE_INDEX_NOT_FOUND, keyframeBoneIndexRight = BONE_INDEX_NOT_FOUND;
	
	// Search for keyframe with an entry for this bone closest to animationTime
	for (keyframeIndex = 0; keyframeIndex < self->keyframeCount && keyframeTime < animationTime; keyframeIndex++) {
		for (unsigned int keyframeBoneIndex = 0; keyframeBoneIndex < self->keyframes[keyframeIndex].boneCount; keyframeBoneIndex++) {
			if (self->keyframes[keyframeIndex].bones[keyframeBoneIndex].boneID == boneID) {
				keyframeIndexLeft = keyframeIndex;
				keyframeTimeLeft = keyframeTime;
				keyframeBoneIndexLeft = keyframeBoneIndex;
				break;
			}
		}
		keyframeTime += self->keyframes[keyframeIndex].interval;
	}
	if (keyframeBoneIndexLeft == BONE_INDEX_NOT_FOUND) {
		// If the bone isn't specified prior to animationTime, find the latest specification past it, if any
		for (; keyframeIndex < self->keyframeCount; keyframeIndex++) {
			for (unsigned int keyframeBoneIndex = 0; keyframeBoneIndex < self->keyframes[keyframeIndex].boneCount; keyframeBoneIndex++) {
				if (self->keyframes[keyframeIndex].bones[keyframeBoneIndex].boneID == boneID) {
					keyframeIndexLeft = keyframeIndex;
					keyframeTimeLeft = keyframeTime;
					keyframeBoneIndexLeft = keyframeBoneIndex;
					break;
				}
			}
			keyframeTime += self->keyframes[keyframeIndex].interval;
		}
	}
	if (keyframeBoneIndexLeft == BONE_INDEX_NOT_FOUND) {
		// This bone isn't specified in this animation
		return false;
	}
	
	// Search for the next appearance of this bone in a keyframe
	keyframeTime = keyframeTimeLeft + self->keyframes[keyframeIndexLeft].interval;
	for (keyframeIndex = keyframeIndexLeft + 1; keyframeIndex < self->keyframeCount; keyframeIndex++) {
		for (unsigned int keyframeBoneIndex = 0; keyframeBoneIndex < self->keyframes[keyframeIndex].boneCount; keyframeBoneIndex++) {
			if (self->keyframes[keyframeIndex].bones[keyframeBoneIndex].boneID == boneID) {
				keyframeIndexRight = keyframeIndex;
				keyframeTimeRight = keyframeTime;
				keyframeBoneIndexRight = keyframeBoneIndex;
				break;
			}
		}
		keyframeTime += self->keyframes[keyframeIndex].interval;
		if (keyframeBoneIndexRight != BONE_INDEX_NOT_FOUND) {
			break;
		}
	}
	if (keyframeBoneIndexRight == BONE_INDEX_NOT_FOUND) {
		// Wrap search around to the beginning if necessary
		for (keyframeIndex = 0; keyframeIndex < keyframeIndexLeft; keyframeIndex++) {
			for (unsigned int keyframeBoneIndex = 0; keyframeBoneIndex < self->keyframes[keyframeIndex].boneCount; keyframeBoneIndex++) {
				if (self->keyframes[keyframeIndex].bones[keyframeBoneIndex].boneID == boneID) {
					keyframeIndexRight = keyframeIndex;
					keyframeTimeRight = keyframeTime;
					keyframeBoneIndexRight = keyframeBoneIndex;
					break;
				}
			}
			keyframeTime += self->keyframes[keyframeIndex].interval;
			if (keyframeBoneIndexRight != BONE_INDEX_NOT_FOUND) {
				break;
			}
		}
	}
	if (keyframeBoneIndexRight == BONE_INDEX_NOT_FOUND) {
		keyframeIndexRight = keyframeIndexLeft;
		keyframeBoneIndexRight = keyframeBoneIndexLeft;
		keyframeTimeRight = keyframeTimeLeft;
	}
	
	*outKeyframeIndexLeft = keyframeIndexLeft;
	*outKeyframeBoneIndexLeft = keyframeBoneIndexLeft;
	*outKeyframeIndexRight = keyframeIndexRight;
	*outKeyframeBoneIndexRight = keyframeBoneIndexRight;
	// TODO: There may be degenerate cases in here. Shouldn't left and right be swapped if animationTime is outside them? Maybe that's impossible?
	if (keyframeTimeLeft == keyframeTimeRight) {
		*outKeyframeWeight = 0.0f;
	} else if (keyframeTimeLeft < keyframeTimeRight) {
		if (animationTime < keyframeTimeLeft) {
			// Degenerate
			*outKeyframeWeight = 0.0f;
		} else if (animationTime < keyframeTimeRight) {
			*outKeyframeWeight = (animationTime - keyframeTimeLeft) / (keyframeTimeRight - keyframeTimeLeft);
		} else {
			// Degenerate
			*outKeyframeWeight = 1.0f;
		}
	} else {
		if (animationTime < keyframeTimeRight) {
			*outKeyframeWeight = (animationTime - (keyframeTimeLeft - animationLength)) / (keyframeTimeRight - (keyframeTimeLeft - animationLength));
		} else if (animationTime < keyframeTimeLeft) {
			// Degenerate
			*outKeyframeWeight = 0.0f;
		} else {
			*outKeyframeWeight = (animationTime - keyframeTimeLeft) / ((keyframeTimeRight + animationLength) - keyframeTimeLeft);
		}
	}
	return true;
}

static inline float curvedKeyframeInterpolationValue(Vector2f leftHandle, Vector2f rightHandle, float value, unsigned int curveSamples) {
	if (leftHandle.x == 0.0f && leftHandle.y == 0.0f && rightHandle.x == 1.0f && rightHandle.y == 1.0f) {
		return value;
	}
	return BezierCurve_sampleYAtX(VECTOR2f(0.0f, 0.0f), leftHandle, rightHandle, VECTOR2f(1.0f, 1.0f), value, curveSamples);
}

void ArmatureAnimation_applyPoseAtTime(ArmatureAnimation * self, ArmaturePose * pose, double animationTime, float weight, unsigned int curveSamples) {
	double animationLength = ArmatureAnimation_getLength(self);
	if (self->loop) {
		animationTime = fmod(animationTime, animationLength);
		if (animationTime < 0.0) {
			animationTime += animationLength;
		}
	}
	
	for (unsigned int poseBoneIndex = 0; poseBoneIndex < pose->poseBoneCount; poseBoneIndex++) {
		unsigned int keyframeIndexLeft = 0, keyframeIndexRight = 0;
		unsigned int keyframeBoneIndexLeft, keyframeBoneIndexRight;
		float keyframeWeight;
		if (findBoneKeyframes(self, pose->poseBones[poseBoneIndex].boneID, animationTime, animationLength, &keyframeIndexLeft, &keyframeBoneIndexLeft, &keyframeIndexRight, &keyframeBoneIndexRight, &keyframeWeight)) {
			// Interpolate bone based on keyframes
			ArmatureAnimationBoneKeyframe boneKeyframeLeft = self->keyframes[keyframeIndexLeft].bones[keyframeBoneIndexLeft];
			ArmatureAnimationBoneKeyframe boneKeyframeRight = self->keyframes[keyframeIndexRight].bones[keyframeBoneIndexRight];
			
			Vector3f boneStateOffset      = Vector3f_interpolate(boneKeyframeLeft.offset,   boneKeyframeRight.offset,   curvedKeyframeInterpolationValue(boneKeyframeLeft.outgoingOffsetBezierHandle,   boneKeyframeRight.incomingOffsetBezierHandle,   keyframeWeight, curveSamples));
			Vector3f boneStateScale       = Vector3f_interpolate(boneKeyframeLeft.scale,    boneKeyframeRight.scale,    curvedKeyframeInterpolationValue(boneKeyframeLeft.outgoingScaleBezierHandle,    boneKeyframeRight.incomingScaleBezierHandle,    keyframeWeight, curveSamples));
			Quaternionf boneStateRotation =    Quaternionf_slerp(boneKeyframeLeft.rotation, boneKeyframeRight.rotation, curvedKeyframeInterpolationValue(boneKeyframeLeft.outgoingRotationBezierHandle, boneKeyframeRight.incomingRotationBezierHandle, keyframeWeight, curveSamples));
			
			pose->poseBones[poseBoneIndex].offset = Vector3f_add(pose->poseBones[poseBoneIndex].offset, Vector3f_multiplyScalar(boneStateOffset, weight));
			pose->poseBones[poseBoneIndex].scale  = Vector3f_multiplyComponents(pose->poseBones[poseBoneIndex].scale, Vector3f_interpolate(VECTOR3f(1.0f, 1.0f, 1.0f), boneStateScale, weight));
			Quaternionf_multiply(&pose->poseBones[poseBoneIndex].rotation, Quaternionf_slerp(QUATERNIONf_IDENTITY, boneStateRotation, weight));
		}
	}
}