#include "gamemath/Armature.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);

static void testInit(void) {
	Armature * armature = Armature_create(0, NULL);
	TestCase_assertPointerNonNULL(armature);
	TestCase_assertUIntEqual(armature->boneCount, 0);
	TestCase_assertPointerNULL(armature->bones);
	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);
	TestCase_assertPointerNonNULL(armature);
	TestCase_assertUIntEqual(armature->boneCount, 2);
	TestCase_assertPointerNonNULL(armature->bones);
	TestCase_assertUIntEqual(armature->bones[0].boneID, 0);
	TestCase_assertUIntEqual(armature->bones[0].parentID, BONE_ID_NONE);
	assertVector3fEqual(armature->bones[0].position, 0.0f, 0.0f, 0.0f);
	assertVector3fEqual(armature->bones[0].endpoint, 1.0f, 0.0f, 0.0f);
	assertQuaternionfEqual(armature->bones[0].baseOrientation, 0.0f, 0.0f, 0.0f, 1.0f);
	TestCase_assertUIntEqual(armature->bones[1].boneID, 1);
	TestCase_assertUIntEqual(armature->bones[1].parentID, 0);
	assertVector3fEqual(armature->bones[1].position, 1.0f, 0.0f, 0.0f);
	assertVector3fEqual(armature->bones[1].endpoint, 1.0f, 1.0f, 0.0f);
	assertQuaternionfEqual(armature->bones[1].baseOrientation, 1.0f, 0.0f, 0.0f, 0.0f);
	Armature_dispose(armature);
}

static void testCopy(void) {
	Armature * armature = Armature_create(0, NULL);
	Armature * copy = Armature_copy(armature);
	Armature_dispose(armature);
	TestCase_assertPointerNonNULL(copy);
	TestCase_assertUIntEqual(copy->boneCount, 0);
	TestCase_assertPointerNULL(copy->bones);
	Armature_dispose(copy);
	
	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);
	copy = Armature_copy(armature);
	TestCase_assertPointerNonNULL(copy);
	TestCase_assertUIntEqual(copy->boneCount, 2);
	TestCase_assertPointerNonNULL(copy->bones);
	TestCase_assertUIntEqual(copy->bones[0].boneID, 0);
	TestCase_assertUIntEqual(copy->bones[0].parentID, BONE_ID_NONE);
	assertVector3fEqual(copy->bones[0].position, 0.0f, 0.0f, 0.0f);
	assertVector3fEqual(copy->bones[0].endpoint, 1.0f, 0.0f, 0.0f);
	assertQuaternionfEqual(copy->bones[0].baseOrientation, 0.0f, 0.0f, 0.0f, 1.0f);
	TestCase_assertUIntEqual(copy->bones[1].boneID, 1);
	TestCase_assertUIntEqual(copy->bones[1].parentID, 0);
	assertVector3fEqual(copy->bones[1].position, 1.0f, 0.0f, 0.0f);
	assertVector3fEqual(copy->bones[1].endpoint, 1.0f, 1.0f, 0.0f);
	assertQuaternionfEqual(copy->bones[1].baseOrientation, 1.0f, 0.0f, 0.0f, 0.0f);
	Armature_dispose(armature);
	Armature_dispose(copy);
}

static void testBoneIndexForID(void) {
	ArmatureBone bones[3] = {
		{5, 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},
		{17, 0, {1.0f, 0.0f, 0.0f}, {1.0f, 1.0f, 0.0f}, {1.0f, 0.0f, 0.0f, 0.0f}, 5},
		{2, 1, {1.0f, 1.5f, 0.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 1.0f, 0.0f, 0.0f}, 17}
	};
	Armature * armature = Armature_create(sizeof_count(bones), bones);
	TestCase_assertUIntEqual(Armature_getBoneIndex(armature, 5), 0);
	TestCase_assertUIntEqual(Armature_getBoneIndex(armature, 17), 1);
	TestCase_assertUIntEqual(Armature_getBoneIndex(armature, 2), 2);
	TestCase_assertUIntEqual(Armature_getBoneIndex(armature, 1), BONE_INDEX_NOT_FOUND);
	Armature_dispose(armature);
}

TEST_SUITE(ArmatureTest,
           testInit,
           testCopy,
           testBoneIndexForID)