#include "modeler/Utilities.h"
#include "unittest/TestSuite.h"
#include <math.h>

static void assertTriangle3fExact(Triangle3f value, Triangle3f expectedValue, int line) {
	if (value.vertex0.x != expectedValue.vertex0.x ||
	    value.vertex0.y != expectedValue.vertex0.y ||
	    value.vertex0.z != expectedValue.vertex0.z ||
	    value.vertex1.x != expectedValue.vertex1.x ||
	    value.vertex1.y != expectedValue.vertex1.y ||
	    value.vertex1.z != expectedValue.vertex1.z ||
	    value.vertex2.x != expectedValue.vertex2.x ||
	    value.vertex2.y != expectedValue.vertex2.y ||
	    value.vertex2.z != expectedValue.vertex2.z) {
		TestCase_assert(false, "Expected triangle {{%f, %f, %f}, {%f, %f, %f}, {%f, %f, %f}} but got triangle {{%f, %f, %f}, {%f, %f, %f}, {%f, %f, %f}} (line %d)",
		                       expectedValue.vertex0.x,
		                       expectedValue.vertex0.y,
		                       expectedValue.vertex0.z,
		                       expectedValue.vertex1.x,
		                       expectedValue.vertex1.y,
		                       expectedValue.vertex1.z,
		                       expectedValue.vertex2.x,
		                       expectedValue.vertex2.y,
		                       expectedValue.vertex2.z,
		                       value.vertex0.x,
		                       value.vertex0.y,
		                       value.vertex0.z,
		                       value.vertex1.x,
		                       value.vertex1.y,
		                       value.vertex1.z,
		                       value.vertex2.x,
		                       value.vertex2.y,
		                       value.vertex2.z,
		                       line);
	}
}

static void assertTriangle3fApproximate(Triangle3f value, Triangle3f expectedValue, float tolerance, int line) {
	if (fabsf(value.vertex0.x - expectedValue.vertex0.x) > tolerance ||
	    fabsf(value.vertex0.y - expectedValue.vertex0.y) > tolerance ||
	    fabsf(value.vertex0.z - expectedValue.vertex0.z) > tolerance ||
	    fabsf(value.vertex1.x - expectedValue.vertex1.x) > tolerance ||
	    fabsf(value.vertex1.y - expectedValue.vertex1.y) > tolerance ||
	    fabsf(value.vertex1.z - expectedValue.vertex1.z) > tolerance ||
	    fabsf(value.vertex2.x - expectedValue.vertex2.x) > tolerance ||
	    fabsf(value.vertex2.y - expectedValue.vertex2.y) > tolerance ||
	    fabsf(value.vertex2.z - expectedValue.vertex2.z) > tolerance) {
		TestCase_assert(false, "Expected triangle {{%f, %f, %f}, {%f, %f, %f}, {%f, %f, %f}} but got triangle {{%f, %f, %f}, {%f, %f, %f}, {%f, %f, %f}} (line %d)",
		                       expectedValue.vertex0.x,
		                       expectedValue.vertex0.y,
		                       expectedValue.vertex0.z,
		                       expectedValue.vertex1.x,
		                       expectedValue.vertex1.y,
		                       expectedValue.vertex1.z,
		                       expectedValue.vertex2.x,
		                       expectedValue.vertex2.y,
		                       expectedValue.vertex2.z,
		                       value.vertex0.x,
		                       value.vertex0.y,
		                       value.vertex0.z,
		                       value.vertex1.x,
		                       value.vertex1.y,
		                       value.vertex1.z,
		                       value.vertex2.x,
		                       value.vertex2.y,
		                       value.vertex2.z,
		                       line);
	}
}

#define TOLERANCE 0.00001f

static void testSplitClippedTriangle(void) {
	Triangle3f splitTriangles[3];
	unsigned int splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                                    VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                                    VECTOR3f(0.0f, 1.0f, 0.0f)), splitTriangles);
	
	// Entire triangle in range
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fExact(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                    VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                    VECTOR3f(0.0f, 1.0f, 0.0f)), __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -1.0f),
	                                                       VECTOR3f(1.0f, 0.0f, -1.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 1.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fExact(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -1.0f),
	                                                    VECTOR3f(1.0f, 0.0f, -1.0f),
	                                                    VECTOR3f(0.0f, 1.0f, 1.0f)), __LINE__);
	
	// Entire triangle out of range
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(0.0f, 1.0f, -2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 0);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 2.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 0);
	
	// One vertex out of range
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(0.0f, 1.0f, -2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 2);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.5f, -1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[1], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.5f, -1.0f),
	                                                          VECTOR3f(0.0f, 0.5f, -1.0f)), TOLERANCE, __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 2);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.5f, 1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[1], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.5f, 1.0f),
	                                                          VECTOR3f(0.0f, 0.5f, 1.0f)), TOLERANCE, __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 0.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 2);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.5f, 0.0f, -1.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.0f, 0.5f, -1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[1], TRIANGLE3f(VECTOR3f(0.0f, 0.5f, -1.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.0f, 1.0f, 0.0f)), TOLERANCE, __LINE__);
	
	// Two vertices out of range in the same direction
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(1.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(0.0f, 1.0f, -2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.0f, -1.0f),
	                                                          VECTOR3f(0.0f, 0.5f, -1.0f)), TOLERANCE, __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 2.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.0f, 1.0f),
	                                                          VECTOR3f(0.0f, 0.5f, 1.0f)), TOLERANCE, __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(0.0f, 1.0f, 0.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.5f, -1.0f),
	                                                          VECTOR3f(0.5f, 0.5f, -1.0f),
	                                                          VECTOR3f(0.0f, 1.0f, 0.0f)), TOLERANCE, __LINE__);
	
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(0.0f, 1.0f, -2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 1);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.5f, 0.0f, -1.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.5f, -1.0f)), TOLERANCE, __LINE__);
	
	// Two vertices out of range in opposite directions
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                       VECTOR3f(1.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(1.0f, 0.0f, 2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 3);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(0.5f, 0.0f, -1.0f),
	                                                          VECTOR3f(1.0f, 0.0f, -1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[1], TRIANGLE3f(VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(1.0f, 0.0f, -1.0f),
	                                                          VECTOR3f(0.5f, 0.0f, 1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[2], TRIANGLE3f(VECTOR3f(0.5f, 0.0f, 1.0f),
	                                                          VECTOR3f(0.0f, 0.0f, 0.0f),
	                                                          VECTOR3f(1.0f, 0.0f, 1.0f)), TOLERANCE, __LINE__);
	// TODO: Different vertex order
	
	// One vertex out of range in one direction, two out of range in the other direction
	splitTriangleCount = splitClippedTriangle3f(TRIANGLE3f(VECTOR3f(1.0f, 0.0f, -2.0f),
	                                                       VECTOR3f(2.0f, 0.0f, 2.0f),
	                                                       VECTOR3f(0.0f, 0.0f, 2.0f)), splitTriangles);
	TestCase_assertUIntEqual(splitTriangleCount, 2);
	assertTriangle3fApproximate(splitTriangles[0], TRIANGLE3f(VECTOR3f(1.25f, 0.0f, -1.0f),
	                                                          VECTOR3f(1.75f, 0.0f, 1.0f),
	                                                          VECTOR3f(0.75f, 0.0f, -1.0f)), TOLERANCE, __LINE__);
	assertTriangle3fApproximate(splitTriangles[1], TRIANGLE3f(VECTOR3f(0.75f, 0.0f, -1.0f),
	                                                          VECTOR3f(1.75f, 0.0f, 1.0f),
	                                                          VECTOR3f(0.25f, 0.0f, 1.0f)), TOLERANCE, __LINE__);
	// TODO: Different vertex order
}

static void testTriangulatePolygon(void) {
	Vector3f positions[7];
	unsigned int indexes[15];
	
	positions[0] = V3f(-1, 0, 0);
	positions[1] = V3f(1, 0, 0);
	positions[2] = V3f(1, 2, 0);
	positions[3] = V3f(0.5f, 1, 0);
	positions[4] = V3f(0, 0.5f, 0);
	positions[5] = V3f(-0.5f, 1, 0);
	positions[6] = V3f(-1, 2, 0);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 7, indexes);
	TestCase_assertUIntEqual(indexes[0], 5);
	TestCase_assertUIntEqual(indexes[1], 6);
	TestCase_assertUIntEqual(indexes[2], 0);
	TestCase_assertUIntEqual(indexes[3], 1);
	TestCase_assertUIntEqual(indexes[4], 2);
	TestCase_assertUIntEqual(indexes[5], 3);
	TestCase_assertUIntEqual(indexes[6], 1);
	TestCase_assertUIntEqual(indexes[7], 3);
	TestCase_assertUIntEqual(indexes[8], 4);
	TestCase_assertUIntEqual(indexes[9], 0);
	TestCase_assertUIntEqual(indexes[10], 1);
	TestCase_assertUIntEqual(indexes[11], 4);
	TestCase_assertUIntEqual(indexes[12], 5);
	TestCase_assertUIntEqual(indexes[13], 0);
	TestCase_assertUIntEqual(indexes[14], 4);
	
	positions[0] = VECTOR3f(0.0f, 0.0f, 0.0f);
	positions[1] = VECTOR3f(1.0f, 0.0f, 0.0f);
	positions[2] = VECTOR3f(0.0f, 1.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 3, indexes);
	TestCase_assertUIntEqual(indexes[0], 0);
	TestCase_assertUIntEqual(indexes[1], 1);
	TestCase_assertUIntEqual(indexes[2], 2);
	
	positions[0] = VECTOR3f(0.0f, 0.0f, 0.0f);
	positions[1] = VECTOR3f(1.0f, 0.0f, 0.0f);
	positions[2] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[3] = VECTOR3f(0.0f, 1.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 4, indexes);
	TestCase_assertUIntEqual(indexes[0], 0);
	TestCase_assertUIntEqual(indexes[1], 1);
	TestCase_assertUIntEqual(indexes[2], 2);
	TestCase_assertUIntEqual(indexes[3], 0);
	TestCase_assertUIntEqual(indexes[4], 2);
	TestCase_assertUIntEqual(indexes[5], 3);
	
	positions[0] = VECTOR3f(-1.0f, -1.0f, 0.0f);
	positions[1] = VECTOR3f(0.0f, 0.5f, 0.0f);
	positions[2] = VECTOR3f(1.0f, -1.0f, 0.0f);
	positions[3] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[4] = VECTOR3f(-1.0f, 1.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 5, indexes);
	TestCase_assertUIntEqual(indexes[0], 4);
	TestCase_assertUIntEqual(indexes[1], 0);
	TestCase_assertUIntEqual(indexes[2], 1);
	TestCase_assertUIntEqual(indexes[3], 3);
	TestCase_assertUIntEqual(indexes[4], 4);
	TestCase_assertUIntEqual(indexes[5], 1);
	TestCase_assertUIntEqual(indexes[6], 2);
	TestCase_assertUIntEqual(indexes[7], 3);
	TestCase_assertUIntEqual(indexes[8], 1);
	
	positions[0] = VECTOR3f(-1.0f, -1.0f, 0.0f);
	positions[1] = VECTOR3f(1.0f, -1.0f, 0.0f);
	positions[2] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[3] = VECTOR3f(0.0f, -0.5f, 0.0f);
	positions[4] = VECTOR3f(-1.0f, 1.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 5, indexes);
	TestCase_assertUIntEqual(indexes[0], 3);
	TestCase_assertUIntEqual(indexes[1], 4);
	TestCase_assertUIntEqual(indexes[2], 0);
	TestCase_assertUIntEqual(indexes[3], 3);
	TestCase_assertUIntEqual(indexes[4], 0);
	TestCase_assertUIntEqual(indexes[5], 1);
	TestCase_assertUIntEqual(indexes[6], 3);
	TestCase_assertUIntEqual(indexes[7], 1);
	TestCase_assertUIntEqual(indexes[8], 2);
	
	positions[0] = VECTOR3f(-1.0f, -1.0f, 0.0f);
	positions[1] = VECTOR3f(1.0f, -1.0f, 0.0f);
	positions[2] = VECTOR3f(0.5f, 0.0f, 0.0f);
	positions[3] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[4] = VECTOR3f(-1.0f, 1.0f, 0.0f);
	positions[5] = VECTOR3f(-0.5f, 0.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 6, indexes);
	TestCase_assertUIntEqual(indexes[0], 5);
	TestCase_assertUIntEqual(indexes[1], 0);
	TestCase_assertUIntEqual(indexes[2], 1);
	TestCase_assertUIntEqual(indexes[3], 5);
	TestCase_assertUIntEqual(indexes[4], 1);
	TestCase_assertUIntEqual(indexes[5], 2);
	TestCase_assertUIntEqual(indexes[6], 5);
	TestCase_assertUIntEqual(indexes[7], 2);
	TestCase_assertUIntEqual(indexes[8], 3);
	TestCase_assertUIntEqual(indexes[9], 5);
	TestCase_assertUIntEqual(indexes[10], 3);
	TestCase_assertUIntEqual(indexes[11], 4);
	
	positions[0] = VECTOR3f(-1.0f, -1.0f, 0.0f);
	positions[1] = VECTOR3f(1.0f, -1.0f, 0.0f);
	positions[2] = VECTOR3f(1.5f, 0.0f, 0.0f);
	positions[3] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[4] = VECTOR3f(-1.0f, 1.0f, 0.0f);
	positions[5] = VECTOR3f(-1.5f, 0.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 6, indexes);
	TestCase_assertUIntEqual(indexes[0], 0);
	TestCase_assertUIntEqual(indexes[1], 1);
	TestCase_assertUIntEqual(indexes[2], 2);
	TestCase_assertUIntEqual(indexes[3], 0);
	TestCase_assertUIntEqual(indexes[4], 2);
	TestCase_assertUIntEqual(indexes[5], 3);
	TestCase_assertUIntEqual(indexes[6], 0);
	TestCase_assertUIntEqual(indexes[7], 3);
	TestCase_assertUIntEqual(indexes[8], 4);
	TestCase_assertUIntEqual(indexes[9], 0);
	TestCase_assertUIntEqual(indexes[10], 4);
	TestCase_assertUIntEqual(indexes[11], 5);
	
	positions[0] = VECTOR3f(-1.0f, -1.0f, 0.0f);
	positions[1] = VECTOR3f(0.5f, -1.25f, 0.0f);
	positions[2] = VECTOR3f(1.0f, -1.0f, 0.0f);
	positions[3] = VECTOR3f(-0.5f, 0.0f, 0.0f);
	positions[4] = VECTOR3f(1.0f, 1.0f, 0.0f);
	positions[5] = VECTOR3f(-1.0f, 1.0f, 0.0f);
	memset(indexes, 0xFF, sizeof(indexes));
	triangulatePolygon(positions, 6, indexes);
	TestCase_assertUIntEqual(indexes[0], 0);
	TestCase_assertUIntEqual(indexes[1], 1);
	TestCase_assertUIntEqual(indexes[2], 2);
	TestCase_assertUIntEqual(indexes[3], 0);
	TestCase_assertUIntEqual(indexes[4], 2);
	TestCase_assertUIntEqual(indexes[5], 3);
	TestCase_assertUIntEqual(indexes[6], 5);
	TestCase_assertUIntEqual(indexes[7], 0);
	TestCase_assertUIntEqual(indexes[8], 3);
	TestCase_assertUIntEqual(indexes[9], 4);
	TestCase_assertUIntEqual(indexes[10], 5);
	TestCase_assertUIntEqual(indexes[11], 3);
}

TEST_SUITE(UtilitiesTest,
           testSplitClippedTriangle,
           testTriangulatePolygon)