#include "bones/BoneAnimationStateController.h"

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

#include "3dmath/Quaternion.h"
#include "bones/BoneAnimationList.h"
#include "bones/BoneAnimationSequence.h"
#include "bones/BoneAnimationState.h"
#include "bones/Skeleton.h"

BoneAnimationStateController * BoneAnimationStateController_create(BoneAnimationState * animationState, BoneAnimationList * animationList) {
	BoneAnimationStateController * self;
	
	self = malloc(sizeof(BoneAnimationStateController));
	BoneAnimationStateController_init(self, animationState, animationList);
	return self;
}

void BoneAnimationStateController_init(BoneAnimationStateController * self, BoneAnimationState * animationState, BoneAnimationList * animationList) {
	self->animationState = animationState;
	self->animationList = animationList;
	self->sequenceName = NULL;
	self->sequencePlaybackTime = 0.0f;
	self->lastAnimationState = NULL;
	self->sequenceTransitionDuration = 1.0f;
	
	self->dispose = BoneAnimationStateController_dispose;
	self->update = BoneAnimationStateController_update;
	self->setSequence = BoneAnimationStateController_setSequence;
}

void BoneAnimationStateController_dispose(void * selfPtr) {
	BoneAnimationStateController * self = selfPtr;
	
	if (self->lastAnimationState != NULL) {
		self->lastAnimationState->dispose(self->lastAnimationState);
	}
	free(self);
}

void BoneAnimationStateController_update(void * selfPtr, double timestep) {
	BoneAnimationStateController * self = selfPtr;
	BoneAnimationSequence * animationSequence;
	unsigned int boneIndex, boneTransformIndex;
	unsigned int keyframeIndex;
	struct BoneAnimationSequence_keyframe * keyframeLeft, * keyframeRight;
	Quaternion orientationLeft, orientationRight;
	Vector3 offsetLeft, offsetRight;
	float normalizedKeyframeTime;
	
	animationSequence = self->animationList->getSequence(self->animationList, self->sequenceName);
	if (animationSequence == NULL || animationSequence->numberOfKeyframes == 0) {
		return;
	}
	
	self->sequencePlaybackTime += timestep;
	
	if (self->lastAnimationState != NULL && self->sequencePlaybackTime > self->sequenceTransitionDuration) {
		self->lastAnimationState->dispose(self->lastAnimationState);
		self->lastAnimationState = NULL;
		self->sequencePlaybackTime = 0.0f;
	}
	
	if (self->lastAnimationState != NULL) {
		keyframeRight = &animationSequence->keyframes[0];
		normalizedKeyframeTime = self->sequencePlaybackTime / self->sequenceTransitionDuration;
		
		for (boneIndex = 0; boneIndex < self->animationState->skeleton->numberOfBones; boneIndex++) {
			orientationLeft = self->lastAnimationState->boneOrientations[boneIndex];
			orientationRight = Quaternion_identity();
			offsetLeft = self->lastAnimationState->boneOffsets[boneIndex];
			offsetRight = Vector3_zero();
			for (boneTransformIndex = 0; boneTransformIndex < keyframeRight->numberOfBones; boneTransformIndex++) {
				if (keyframeRight->boneTransforms[boneTransformIndex].boneID == self->animationState->skeleton->bones[boneIndex].boneID) {
					orientationRight = keyframeRight->boneTransforms[boneTransformIndex].orientation;
					offsetRight = keyframeRight->boneTransforms[boneTransformIndex].offset;
				}
			}
			self->animationState->boneOrientations[boneIndex] = Quaternion_slerp(orientationLeft, orientationRight, normalizedKeyframeTime);
			self->animationState->boneOffsets[boneIndex] = Vector3_interpolate(offsetLeft, offsetRight, normalizedKeyframeTime);
		}
		
	} else {
		self->sequencePlaybackTime = fmod(self->sequencePlaybackTime, animationSequence->getDuration(animationSequence));
		keyframeIndex = animationSequence->keyframeIndexForTime(animationSequence, self->sequencePlaybackTime);
		keyframeLeft = &animationSequence->keyframes[keyframeIndex];
		keyframeRight = &animationSequence->keyframes[(keyframeIndex + 1) % animationSequence->numberOfKeyframes];
		normalizedKeyframeTime = (self->sequencePlaybackTime - animationSequence->timeForKeyframeIndex(animationSequence, keyframeIndex)) / keyframeLeft->duration;
		
		for (boneIndex = 0; boneIndex < self->animationState->skeleton->numberOfBones; boneIndex++) {
			orientationLeft = orientationRight = Quaternion_identity();
			offsetLeft = offsetRight = Vector3_zero();
			for (boneTransformIndex = 0; boneTransformIndex < keyframeLeft->numberOfBones; boneTransformIndex++) {
				if (keyframeLeft->boneTransforms[boneTransformIndex].boneID == self->animationState->skeleton->bones[boneIndex].boneID) {
					orientationLeft = keyframeLeft->boneTransforms[boneTransformIndex].orientation;
					offsetLeft = keyframeLeft->boneTransforms[boneTransformIndex].offset;
				}
			}
			for (boneTransformIndex = 0; boneTransformIndex < keyframeRight->numberOfBones; boneTransformIndex++) {
				if (keyframeRight->boneTransforms[boneTransformIndex].boneID == self->animationState->skeleton->bones[boneIndex].boneID) {
					orientationRight = keyframeRight->boneTransforms[boneTransformIndex].orientation;
					offsetRight = keyframeRight->boneTransforms[boneTransformIndex].offset;
				}
			}
			self->animationState->boneOrientations[boneIndex] = Quaternion_slerp(orientationLeft, orientationRight, normalizedKeyframeTime);
			self->animationState->boneOffsets[boneIndex] = Vector3_interpolate(offsetLeft, offsetRight, normalizedKeyframeTime);
		}
	}
	
	self->animationState->updateBoneMatrices(self->animationState);
}

void BoneAnimationStateController_setSequence(void * selfPtr, Atom sequenceName) {
	BoneAnimationStateController * self = selfPtr;
	BoneAnimationSequence * animationSequence;
	
	if (sequenceName != self->sequenceName) {
		self->lastAnimationState = self->animationState->createCopy(self->animationState);
		
		animationSequence = self->animationList->getSequence(self->animationList, self->sequenceName);
		if (animationSequence != NULL) {
			struct BoneAnimationSequence_keyframe * keyframe;
			
			keyframe = &animationSequence->keyframes[animationSequence->keyframeIndexForTime(animationSequence, self->sequencePlaybackTime)];
			self->sequenceTransitionDuration = keyframe->duration;
			animationSequence = self->animationList->getSequence(self->animationList, sequenceName);
			if (animationSequence != NULL) {
				keyframe = &animationSequence->keyframes[0];
				if (keyframe->duration < self->sequenceTransitionDuration) {
					self->sequenceTransitionDuration = keyframe->duration;
				}
			}
		}
		
		self->sequenceName = sequenceName;
		self->sequencePlaybackTime = 0.0f;
	}
}