#include "gamemath/ArmatureAnimation.h"
#include "unittest/TestSuite.h"
#include "stem_core.h"

#define TEST_EPSILON 0.00001f

#define assertVector2fEqual(vector, expected_x, expected_y) \
	TestCase_assert((vector).x == (expected_x) && (vector).y == (expected_y), "Expected {%f, %f} but got {%f, %f}", (expected_x), (expected_y), (vector).x, (vector).y);

#define assertVector3fEqual(vector, expected_x, expected_y, expected_z) \
	TestCase_assert((vector).x == (expected_x) && (vector).y == (expected_y) && (vector).z == (expected_z), "Expected {%f, %f, %f} but got {%f, %f, %f}", (expected_x), (expected_y), (expected_z), (vector).x, (vector).y, (vector).z);

#define assertQuaternionfEqual(quaternion, expected_x, expected_y, expected_z, expected_w) \
	TestCase_assert((quaternion).x == (expected_x) && (quaternion).y == (expected_y) && (quaternion).z == (expected_z) && (quaternion).w == (expected_w), "Expected {%f, %f, %f, %f} but got {%f, %f, %f, %f}", (expected_x), (expected_y), (expected_z), (expected_w), (quaternion).x, (quaternion).y, (quaternion).z, (quaternion).w);

#define assertQuaternionfApproximate(quaternion, expected_x, expected_y, expected_z, expected_w, epsilon) \
	TestCase_assert(fabsf((quaternion).x - (expected_x)) < (epsilon) && \
	                fabsf((quaternion).y - (expected_y)) < (epsilon) && \
	                fabsf((quaternion).z - (expected_z)) < (epsilon) && \
	                fabsf((quaternion).w - (expected_w)) < (epsilon), \
	                "Expected {%f, %f, %f, %f} but got {%f, %f, %f, %f}", (expected_x), (expected_y), (expected_z), (expected_w), (quaternion).x, (quaternion).y, (quaternion).z, (quaternion).w);

static void testInit(void) {
	ArmatureAnimationKeyframe keyframe = {1.0, 0, NULL};
	ArmatureAnimation * animation = ArmatureAnimation_create(false, 1, &keyframe, 0, NULL);
	TestCase_assertPointerNonNULL(animation);
	TestCase_assertBoolFalse(animation->loop);
	TestCase_assertUIntEqual(animation->keyframeCount, 1);
	TestCase_assertPointerNonNULL(animation->keyframes);
	TestCase_assertPointerUnequal(animation->keyframes, &keyframe);
	TestCase_assertDoubleEqual(animation->keyframes[0].interval, 1.0);
	TestCase_assertUIntEqual(animation->keyframes[0].boneCount, 0);
	TestCase_assertPointerNULL(animation->keyframes[0].bones);
	TestCase_assertUIntEqual(animation->markerCount, 0);
	TestCase_assertPointerNULL(animation->markers);
	ArmatureAnimation_dispose(animation);
	
	ArmatureAnimationBoneKeyframe boneKeyframes1[] = {
		{0, {0.25f, 0.5f, 0.75f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {1.0f, 1.25f, 1.5f}, {0.125f, 0.25f}, {0.75f, 0.875f}, {1.0f, 0.0f, 0.0f, 0.0f}, {0.0f, 0.25f}, {0.5f, 1.0f}},
		{1, {1.0f, 0.0f, 0.25f}, {0.125f, 0.125f}, {0.875f, 0.75f}, {0.5f, 0.25f, 1.0f}, {0.25f, 0.5f}, {0.5f, 0.75f}, {0.0f, 0.5f, 0.5f, 0.5f}, {1.0f, 1.0f}, {0.0f, 0.0f}}
	};
	ArmatureAnimationBoneKeyframe boneKeyframes2[] = {
		{2, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}}
	};
	ArmatureAnimationKeyframe keyframes[] = {
		{0.5, sizeof_count(boneKeyframes1), boneKeyframes1},
		{1.25, sizeof_count(boneKeyframes2), boneKeyframes2}
	};
	ArmatureAnimationMarker markers[] = {
		{0, 0.25},
		{1, 1.0}
	};
	animation = ArmatureAnimation_create(true, sizeof_count(keyframes), keyframes, sizeof_count(markers), markers);
	TestCase_assertPointerNonNULL(animation);
	TestCase_assertBoolTrue(animation->loop);
	TestCase_assertUIntEqual(animation->keyframeCount, 2);
	TestCase_assertPointerNonNULL(animation->keyframes);
	TestCase_assertPointerUnequal(animation->keyframes, keyframes);
	
	TestCase_assertDoubleEqual(animation->keyframes[0].interval, 0.5);
	TestCase_assertUIntEqual(animation->keyframes[0].boneCount, 2);
	TestCase_assertPointerNonNULL(animation->keyframes[0].bones);
	TestCase_assertPointerUnequal(animation->keyframes[0].bones, keyframes[0].bones);
	TestCase_assertUIntEqual(animation->keyframes[0].bones[0].boneID, 0);
	assertVector3fEqual(animation->keyframes[0].bones[0].offset, 0.25f, 0.5f, 0.75f);
	assertVector2fEqual(animation->keyframes[0].bones[0].incomingOffsetBezierHandle, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[0].bones[0].outgoingOffsetBezierHandle, 1.0f, 1.0f);
	assertVector3fEqual(animation->keyframes[0].bones[0].scale, 1.0f, 1.25f, 1.5f);
	assertVector2fEqual(animation->keyframes[0].bones[0].incomingScaleBezierHandle, 0.125f, 0.25f);
	assertVector2fEqual(animation->keyframes[0].bones[0].outgoingScaleBezierHandle, 0.75f, 0.875f);
	assertQuaternionfEqual(animation->keyframes[0].bones[0].rotation, 1.0f, 0.0f, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[0].bones[0].incomingRotationBezierHandle, 0.0f, 0.25f);
	assertVector2fEqual(animation->keyframes[0].bones[0].outgoingRotationBezierHandle, 0.5f, 1.0f);
	TestCase_assertUIntEqual(animation->keyframes[0].bones[1].boneID, 1);
	assertVector3fEqual(animation->keyframes[0].bones[1].offset, 1.0f, 0.0f, 0.25f);
	assertVector2fEqual(animation->keyframes[0].bones[1].incomingOffsetBezierHandle, 0.125f, 0.125f);
	assertVector2fEqual(animation->keyframes[0].bones[1].outgoingOffsetBezierHandle, 0.875f, 0.75f);
	assertVector3fEqual(animation->keyframes[0].bones[1].scale, 0.5f, 0.25f, 1.0f);
	assertVector2fEqual(animation->keyframes[0].bones[1].incomingScaleBezierHandle, 0.25f, 0.5f);
	assertVector2fEqual(animation->keyframes[0].bones[1].outgoingScaleBezierHandle, 0.5f, 0.75f);
	assertQuaternionfEqual(animation->keyframes[0].bones[1].rotation, 0.0f, 0.5f, 0.5f, 0.5f);
	assertVector2fEqual(animation->keyframes[0].bones[1].incomingRotationBezierHandle, 1.0f, 1.0f);
	assertVector2fEqual(animation->keyframes[0].bones[1].outgoingRotationBezierHandle, 0.0f, 0.0f);
	
	TestCase_assertDoubleEqual(animation->keyframes[1].interval, 1.25);
	TestCase_assertUIntEqual(animation->keyframes[1].boneCount, 1);
	TestCase_assertPointerNonNULL(animation->keyframes[1].bones);
	TestCase_assertPointerUnequal(animation->keyframes[1].bones, keyframes[1].bones);
	TestCase_assertUIntEqual(animation->keyframes[1].bones[0].boneID, 2);
	assertVector3fEqual(animation->keyframes[1].bones[0].offset, 0.0f, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].incomingOffsetBezierHandle, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].outgoingOffsetBezierHandle, 1.0f, 1.0f);
	assertVector3fEqual(animation->keyframes[1].bones[0].scale, 1.0f, 1.0f, 1.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].incomingScaleBezierHandle, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].outgoingScaleBezierHandle, 1.0f, 1.0f);
	assertQuaternionfEqual(animation->keyframes[1].bones[0].rotation, 0.0f, 0.0f, 0.0f, 1.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].incomingRotationBezierHandle, 0.0f, 0.0f);
	assertVector2fEqual(animation->keyframes[1].bones[0].outgoingRotationBezierHandle, 1.0f, 1.0f);
	
	TestCase_assertUIntEqual(animation->markerCount, 2);
	TestCase_assertPointerNonNULL(animation->markers);
	TestCase_assertPointerUnequal(animation->markers, markers);
	TestCase_assertUIntEqual(animation->markers[0].identifier, 0);
	TestCase_assertDoubleEqual(animation->markers[0].time, 0.25);
	TestCase_assertUIntEqual(animation->markers[1].identifier, 1);
	TestCase_assertDoubleEqual(animation->markers[1].time, 1.0);
	ArmatureAnimation_dispose(animation);
}

static void testGetLength(void) {
	ArmatureAnimationKeyframe keyframes[] = {
		{0.5, 0, NULL},
		{1.25, 0, NULL}
	};
	ArmatureAnimation * animation = ArmatureAnimation_create(true, sizeof_count(keyframes), keyframes, 0, NULL);
	TestCase_assertDoubleEqual(ArmatureAnimation_getLength(animation), 1.75);
	animation->keyframes[0].interval = 1.0;
	TestCase_assertDoubleEqual(ArmatureAnimation_getLength(animation), 2.25);
	ArmatureAnimation_dispose(animation);
}

static void testGetMarkerAtTime(void) {
	ArmatureAnimationKeyframe keyframes[] = {
		{0.5, 0, NULL},
		{1.0, 0, NULL}
	};
	ArmatureAnimationMarker markers[] = {
		{0, 0.25},
		{1, 1.0}
	};
	ArmatureAnimation * animation = ArmatureAnimation_create(false, sizeof_count(keyframes), keyframes, sizeof_count(markers), markers);
	ArmatureAnimationMarker * marker = ArmatureAnimation_getMarkerAtTime(animation, 0.0);
	TestCase_assertPointerNULL(marker);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 0.25);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 0.9999);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.0);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.7499);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.75);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, -0.5);
	TestCase_assertPointerNULL(marker);
	marker = ArmatureAnimation_getMarkerAtTime(animation, -1.0);
	TestCase_assertPointerNULL(marker);
	
	// TODO: Check correctness of loop behavior with negative values after applyPoseAtTime changes have been made
	animation->loop = true;
	marker = ArmatureAnimation_getMarkerAtTime(animation, 0.0);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 0.25);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 0.9999);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.0);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.7499);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, 1.75);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	marker = ArmatureAnimation_getMarkerAtTime(animation, -0.5);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 1);
	TestCase_assertDoubleEqual(marker->time, 1.0);
	marker = ArmatureAnimation_getMarkerAtTime(animation, -1.0);
	TestCase_assertPointerNonNULL(marker);
	TestCase_assertUIntEqual(marker->identifier, 0);
	TestCase_assertDoubleEqual(marker->time, 0.25);
	ArmatureAnimation_dispose(animation);
}

static void testApplyPoseAtTime(void) {
	// TODO: This is a lot more complicated than this. Non-looping animations are definitely wrong. Looping might be wrong too.
	// First keyframe should interpolate from identity orientations to what it specifies over its interval. If an initial snap
	// is needed, interval is 0. Don't pull in ending keyframes from beginning; only interpolate from end to beginning if time
	// is > animation length. findBoneKeyframes will need a lot of work.
	// Test bezier interpolation, weight argument, multiple applications, different sets of specified bones per keyframe
	// separately from loop bounds behavior.
	/*ArmatureBone bones[2] = {
		{ATOM("a"), BONE_INDEX_NOT_FOUND, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}},
		{ATOM("b"), 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	ArmaturePose * pose = ArmaturePose_create(armature);
	
	ArmatureAnimationBoneKeyframe boneKeyframes1[] = {
		{ATOM("a"), {0.25f, 0.5f, 0.75f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {1.0f, 1.25f, 1.5f}, {0.125f, 0.25f}, {0.75f, 0.875f}, {1.0f, 0.0f, 0.0f, 0.0f}, {0.0f, 0.25f}, {0.5f, 1.0f}},
		{ATOM("b"), {1.0f, 0.0f, 0.25f}, {0.125f, 0.125f}, {0.875f, 0.75f}, {0.5f, 0.25f, 1.0f}, {0.25f, 0.5f}, {0.5f, 0.75f}, {0.0f, 0.5f, 0.5f, 0.5f}, {1.0f, 1.0f}, {0.0f, 0.0f}}
	};
	ArmatureAnimationBoneKeyframe boneKeyframes2[] = {
		{ATOM("a"), {1.25f, 1.5f, 1.75f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {1.5f, 1.75f, 2.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {0.0f, 0.0f, 1.0f, 0.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}},
		{ATOM("b"), {0.0f, -1.0f, -0.75f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {0.25f, 0.125f, 0.5f}, {0.0f, 0.0f}, {1.0f, 1.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f}, {1.0f, 1.0f}}
	};
	ArmatureAnimationKeyframe keyframes[] = {
		{0.5, sizeof_count(boneKeyframes1), boneKeyframes1},
		{1.0, sizeof_count(boneKeyframes2), boneKeyframes2}
	};
	ArmatureAnimation * animation = ArmatureAnimation_create(false, sizeof_count(keyframes), keyframes, 0, NULL);
	
	ArmatureAnimation_applyPoseAtTime(animation, pose, 0.0, 0.0);
	assertVector3fEqual(pose->poseBones[0].offset, 0.0f, 0.0f, 0.0f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 1.0f, 1.0f);
	assertQuaternionfEqual(pose->poseBones[0].rotation, 0.0f, 0.0f, 0.0f, 1.0f);
	assertVector3fEqual(pose->poseBones[1].offset, 0.0f, 0.0f, 0.0f);
	assertVector3fEqual(pose->poseBones[1].scale, 1.0f, 1.0f, 1.0f);
	assertQuaternionfEqual(pose->poseBones[1].rotation, 0.0f, 0.0f, 0.0f, 1.0f);
	
	ArmatureAnimation_applyPoseAtTime(animation, pose, 0.0, 0.5);
	assertVector3fEqual(pose->poseBones[0].offset, 0.125f, 0.25f, 0.375f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 1.125f, 1.25f);
	assertQuaternionfApproximate(pose->poseBones[0].rotation, 0.707107f, 0.0f, 0.0f, 0.707107f, TEST_EPSILON);
	assertVector3fEqual(pose->poseBones[1].offset, 0.5f, 0.0f, 0.125f);
	assertVector3fEqual(pose->poseBones[1].scale, 0.75f, 0.625f, 1.0f);
	assertQuaternionfApproximate(pose->poseBones[1].rotation, 0.0f, 0.325058f, 0.325058f, 0.888074f, TEST_EPSILON);
	
	ArmatureAnimation_applyPoseAtTime(animation, pose, 0.0, 0.5);
	assertVector3fEqual(pose->poseBones[0].offset, 0.25f, 0.5f, 0.75f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 1.265625f, 1.5625f);
	assertQuaternionfApproximate(pose->poseBones[0].rotation, 1.0f, 0.0f, 0.0f, 0.0f, TEST_EPSILON);
	assertVector3fEqual(pose->poseBones[1].offset, 1.0f, 0.0f, 0.25f);
	assertVector3fEqual(pose->poseBones[1].scale, 0.562500f, 0.390625f, 1.0f);
	assertQuaternionfApproximate(pose->poseBones[1].rotation, 0.0f, 0.577350f, 0.577350f, 0.577350f, TEST_EPSILON);
	
	ArmaturePose_resetAllPoseBones(pose);
	ArmatureAnimation_applyPoseAtTime(animation, pose, 0.0, 1.0);
	assertVector3fEqual(pose->poseBones[0].offset, 0.25f, 0.5f, 0.75f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 1.25f, 1.5f);
	assertQuaternionfApproximate(pose->poseBones[0].rotation, 1.0f, 0.0f, 0.0f, 0.0f, TEST_EPSILON);
	assertVector3fEqual(pose->poseBones[1].offset, 1.0f, 0.0f, 0.25f);
	assertVector3fEqual(pose->poseBones[1].scale, 0.5f, 0.25f, 1.0f);
	assertQuaternionfApproximate(pose->poseBones[1].rotation, 0.0f, 0.577350f, 0.577350f, 0.577350f, TEST_EPSILON);
	
	ArmaturePose_resetAllPoseBones(pose);
	ArmatureAnimation_applyPoseAtTime(animation, pose, 0.5, 1.0);
	assertVector3fEqual(pose->poseBones[0].offset, 1.25f, 1.5f, 1.75f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.5f, 1.75f, 2.0f);
	assertQuaternionfApproximate(pose->poseBones[0].rotation, 0.0f, 0.0f, 1.0f, 0.0f, TEST_EPSILON);
	assertVector3fEqual(pose->poseBones[1].offset, 0.0f, -1.0f, -0.75f);
	assertVector3fEqual(pose->poseBones[1].scale, 0.25f, 0.125f, 0.5f);
	assertQuaternionfApproximate(pose->poseBones[1].rotation, 0.0f, 0.0f, 0.0f, 1.0f, TEST_EPSILON);
	
	ArmatureAnimation_dispose(animation);
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);*/
}

TEST_SUITE(ArmatureAnimationTest,
           testInit,
           testGetLength,
           testGetMarkerAtTime,
           testApplyPoseAtTime)