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

#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 assertMatrix4x4fEqual(matrix, expected_0, expected_4, expected_8, expected_12, expected_1, expected_5, expected_9, expected_13, expected_2, expected_6, expected_10, expected_14, expected_3, expected_7, expected_11, expected_15) \
	TestCase_assert((matrix).m[0] == (expected_0) && \
	                (matrix).m[1] == (expected_1) && \
	                (matrix).m[2] == (expected_2) && \
	                (matrix).m[3] == (expected_3) && \
	                (matrix).m[4] == (expected_4) && \
	                (matrix).m[5] == (expected_5) && \
	                (matrix).m[6] == (expected_6) && \
	                (matrix).m[7] == (expected_7) && \
	                (matrix).m[8] == (expected_8) && \
	                (matrix).m[9] == (expected_9) && \
	                (matrix).m[10] == (expected_10) && \
	                (matrix).m[11] == (expected_11) && \
	                (matrix).m[12] == (expected_12) && \
	                (matrix).m[13] == (expected_13) && \
	                (matrix).m[14] == (expected_14) && \
	                (matrix).m[15] == (expected_15), \
	                "Expected {\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f\n} but got {\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f,\n\t%f, %f, %f, %f\n}", (expected_0), (expected_4), (expected_8), (expected_12), (expected_1), (expected_5), (expected_9), (expected_13), (expected_2), (expected_6), (expected_10), (expected_14), (expected_3), (expected_7), (expected_11), (expected_15), (matrix).m[0], (matrix).m[4], (matrix).m[8], (matrix).m[12], (matrix).m[1], (matrix).m[5], (matrix).m[9], (matrix).m[13], (matrix).m[2], (matrix).m[6], (matrix).m[10], (matrix).m[14], (matrix).m[3], (matrix).m[7], (matrix).m[11], (matrix).m[15]);

static void testInit(void) {
	Armature * armature = Armature_create(0, NULL);
	ArmaturePose * pose = ArmaturePose_createWithArmature(armature);
	TestCase_assertPointerNonNULL(pose);
	TestCase_assertPointerNULL(pose->poseBones);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 0);
	TestCase_assertPointerNULL(pose->poseBoneTransforms);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 0);
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
	
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	armature = Armature_create(sizeof_count(bones), bones);
	pose = ArmaturePose_createWithArmature(armature);
	TestCase_assertPointerNonNULL(pose);
	TestCase_assertPointerNonNULL(pose->poseBones);
	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);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 0);
	TestCase_assertPointerNULL(pose->poseBoneTransforms);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 0);
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
}

static void testCopy(void) {
	Armature * armature = Armature_create(0, NULL);
	ArmaturePose * original = ArmaturePose_createWithArmature(armature);
	ArmaturePose * pose = ArmaturePose_copy(original);
	ArmaturePose_dispose(original);
	TestCase_assertPointerNonNULL(pose);
	TestCase_assertPointerNULL(pose->poseBones);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 0);
	TestCase_assertPointerNULL(pose->poseBoneTransforms);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 0);
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
	
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	armature = Armature_create(sizeof_count(bones), bones);
	original = ArmaturePose_createWithArmature(armature);
	pose = ArmaturePose_copy(original);
	ArmaturePose_dispose(original);
	TestCase_assertPointerNonNULL(pose);
	TestCase_assertPointerNonNULL(pose->poseBones);
	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);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 0);
	TestCase_assertPointerNULL(pose->poseBoneTransforms);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 0);
	ArmaturePose_dispose(pose);
	
	original = ArmaturePose_createWithArmature(armature);
	original->poseBones[0].offset.x = 1.0f;
	original->poseBones[0].scale.y = 1.5f;
	original->poseBones[0].rotation.z = 1.0f;
	original->poseBones[1].offset.y = 0.5f;
	original->poseBones[1].scale.z = 0.5f;
	original->poseBones[1].rotation.w = 0.5f;
	original->poseTransformDirtyValue = 4;
	original->poseBoneTransformCount = 1;
	original->poseBoneTransforms = malloc(sizeof(*original->poseBoneTransforms));
	original->poseBoneTransforms[0] = MATRIX4x4f_IDENTITY;
	original->poseBoneTransforms[0].m[12] = 1.0f;
	pose = ArmaturePose_copy(original);
	ArmaturePose_dispose(original);
	TestCase_assertPointerNonNULL(pose);
	TestCase_assertPointerNonNULL(pose->poseBones);
	assertVector3fEqual(pose->poseBones[0].offset, 1.0f, 0.0f, 0.0f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 1.5f, 1.0f);
	assertQuaternionfEqual(pose->poseBones[0].rotation, 0.0f, 0.0f, 1.0f, 1.0f);
	assertVector3fEqual(pose->poseBones[1].offset, 0.0f, 0.5f, 0.0f);
	assertVector3fEqual(pose->poseBones[1].scale, 1.0f, 1.0f, 0.5f);
	assertQuaternionfEqual(pose->poseBones[1].rotation, 0.0f, 0.0f, 0.0f, 0.5f);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 1);
	TestCase_assertPointerNonNULL(pose->poseBoneTransforms);
	assertMatrix4x4fEqual(pose->poseBoneTransforms[0], 1.0f, 0.0f, 0.0f, 1.0f,
	                                                   0.0f, 1.0f, 0.0f, 0.0f,
	                                                   0.0f, 0.0f, 1.0f, 0.0f,
	                                                   0.0f, 0.0f, 0.0f, 1.0f);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 4);
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
}

static void testResetAllPoseBones(void) {
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	ArmaturePose * pose = ArmaturePose_createWithArmature(armature);
	pose->poseBones[0].offset.x = 1.0f;
	pose->poseBones[0].scale.y = 1.5f;
	pose->poseBones[0].rotation.z = 1.0f;
	pose->poseBones[1].offset.y = 0.5f;
	pose->poseBones[1].scale.z = 0.5f;
	pose->poseBones[1].rotation.w = 0.5f;
	
	ArmaturePose_resetAllPoseBones(pose);
	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);
	
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
}

static void testComputePoseBoneTransforms(void) {
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	ArmaturePose * pose = ArmaturePose_createWithArmature(armature);
	pose->poseBones[0].offset.x = 1.0f;
	pose->poseBones[0].scale.y = 1.5f;
	pose->poseBones[0].rotation.z = 1.0f;
	pose->poseBones[1].offset.y = 0.5f;
	pose->poseBones[1].scale.z = 0.5f;
	pose->poseBones[1].rotation.x = 0.5f;
	ArmaturePose_computePoseBoneTransforms(pose, armature);
	TestCase_assertUIntEqual(pose->poseBoneTransformCount, 2);
	assertMatrix4x4fEqual(pose->poseBoneTransforms[0], -1.0f, -2.0f,  0.0f,  1.0f,
	                                                     3.0f, -1.5f,  0.0f,  0.0f,
	                                                     0.0f,  0.0f,  1.0f,  0.0f,
	                                                     0.0f,  0.0f,  0.0f,  1.0f);
	assertMatrix4x4fEqual(pose->poseBoneTransforms[1], -1.0f, -1.0f,  2.0f,  0.0f,
	                                                     3.0f, -0.75f, 1.5f, -0.75f,
	                                                     0.0f,  0.5f,  0.25f, 0.0f,
	                                                     0.0f,  0.0f,  0.0f,  1.0f);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 1);
	
	ArmaturePose_computePoseBoneTransforms(pose, armature);
	TestCase_assertUIntEqual(pose->poseTransformDirtyValue, 2);
	
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
}

static void testGetPoseBonePositionAndEndpoint(void) {
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	ArmaturePose * pose = ArmaturePose_createWithArmature(armature);
	pose->poseBones[0].offset.x = 1.0f;
	pose->poseBones[0].scale.y = 1.5f;
	pose->poseBones[0].rotation.z = 1.0f;
	pose->poseBones[1].offset.y = 0.5f;
	pose->poseBones[1].scale.z = 0.5f;
	pose->poseBones[1].rotation.x = 0.5f;
	ArmaturePose_computePoseBoneTransforms(pose, armature);
	
	Vector3f position = ArmaturePose_getPoseBonePosition(pose, 0, armature);
	assertVector3fEqual(position, 1.0f, 0.0f, 0.0f);
	Vector3f endpoint = ArmaturePose_getPoseBoneEndpoint(pose, 0, armature);
	assertVector3fEqual(endpoint, 0.0f, 3.0f, 0.0f);
	position = ArmaturePose_getPoseBonePosition(pose, 1, armature);
	assertVector3fEqual(position, -1.0f, 2.25f, 0.0f);
	endpoint = ArmaturePose_getPoseBoneEndpoint(pose, 1, armature);
	assertVector3fEqual(endpoint, -2.0f, 1.5f, 0.5f);
	
	ArmaturePose_dispose(pose);
	Armature_dispose(armature);
}

static void testCombine(void) {
	ArmatureBone bones[2] = {
		{0, BONE_ID_NONE, {0.0f, 0.0f, 0.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 1.0f}, BONE_INDEX_NOT_FOUND},
		{1, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 0}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	ArmaturePose * pose = ArmaturePose_createWithArmature(armature);
	pose->poseBones[0].offset.x = 1.0f;
	pose->poseBones[0].scale.y = 1.5f;
	pose->poseBones[0].rotation.z = 1.0f;
	pose->poseBones[1].offset.y = 0.5f;
	pose->poseBones[1].scale.z = 0.5f;
	pose->poseBones[1].rotation.x = 0.5f;
	ArmaturePose * pose2 = ArmaturePose_createWithArmature(armature);
	pose2->poseBones[0].offset.x = 0.5f;
	pose2->poseBones[0].offset.y = 1.0f;
	pose2->poseBones[0].scale.y = 0.5f;
	pose2->poseBones[0].scale.z = 1.5f;
	pose2->poseBones[0].rotation.x = 1.0f;
	pose2->poseBones[1].offset.z = 0.5f;
	pose2->poseBones[1].scale.x = 0.5f;
	pose2->poseBones[1].rotation.y = 0.5f;
	
	ArmaturePose_combinePoses(pose, pose2);
	assertVector3fEqual(pose->poseBones[0].offset, 1.5f, 1.0f, 0.0f);
	assertVector3fEqual(pose->poseBones[0].scale, 1.0f, 0.75f, 1.5f);
	assertQuaternionfEqual(pose->poseBones[0].rotation, 1.0f, 1.0f, 1.0f, 1.0f);
	assertVector3fEqual(pose->poseBones[1].offset, 0.0f, 0.5f, 0.5f);
	assertVector3fEqual(pose->poseBones[1].scale, 0.5f, 1.0f, 0.5f);
	assertQuaternionfEqual(pose->poseBones[1].rotation, 0.5f, 0.5f, 0.25f, 1.0f);
	
	ArmaturePose_dispose(pose);
	ArmaturePose_dispose(pose2);
	Armature_dispose(armature);
}

TEST_SUITE(ArmaturePoseTest,
           testInit,
           testCopy,
           testResetAllPoseBones,
           testComputePoseBoneTransforms,
           testGetPoseBonePositionAndEndpoint,
           testCombine)