#include "collision/IntersectionManager.h"
#include "collision/StandardIntersectionHandlers.h"
#include "unittest/TestSuite.h"

static void verifyInit(int line, IntersectionManager * intersectionManager) {
	TestCase_assert(intersectionManager != NULL, "Expected non-NULL but got NULL (line %d)", line);
	TestCase_assert(intersectionManager->vtable->dispose == IntersectionManager_dispose, "Expected %p but got %p (line %d)", IntersectionManager_dispose, intersectionManager->vtable->dispose, line);
	TestCase_assert(intersectionManager->defaultCollisionTestHandler == NULL, "Expected NULL but got %p (line %d)", intersectionManager->defaultCollisionTestHandler, line);
	TestCase_assert(intersectionManager->defaultOverlapTestHandler == NULL, "Expected NULL but got %p (line %d)", intersectionManager->defaultOverlapTestHandler, line);
}

static void testInit() {
	IntersectionManager intersectionManager, * intersectionManagerPtr;
	
	memset(&intersectionManager, 0x00, sizeof(intersectionManager));
	stemobject_assign_vtable(intersectionManager, IntersectionManager);
	IntersectionManager_init(&intersectionManager);
	verifyInit(__LINE__, &intersectionManager);
	IntersectionManager_dispose(&intersectionManager);
	
	memset(&intersectionManager, 0xFF, sizeof(intersectionManager));
	stemobject_assign_vtable(intersectionManager, IntersectionManager);
	IntersectionManager_init(&intersectionManager);
	verifyInit(__LINE__, &intersectionManager);
	IntersectionManager_dispose(&intersectionManager);
	
	intersectionManagerPtr = IntersectionManager_create();
	verifyInit(__LINE__, intersectionManagerPtr);
	IntersectionManager_dispose(intersectionManagerPtr);
}

static unsigned int handler1Calls, handler2Calls;

static bool testHandler1(CollisionObject * object1, CollisionObject * object2, fixed16_16 * outTime, Vector3x * outNormal, Vector3x * outObject1Vector, Vector3x * outObject2Vector, fixed16_16 * outContactArea) {
	handler1Calls++;
	return false;
}

static bool testHandler2(CollisionObject * object1, CollisionObject * object2, fixed16_16 * outTime, Vector3x * outNormal, Vector3x * outObject1Vector, Vector3x * outObject2Vector, fixed16_16 * outContactArea) {
	handler2Calls++;
	*outTime = 1;
	*outNormal = VECTOR3x(2, 3, 4);
	*outObject1Vector = VECTOR3x(5, 6, 7);
	*outObject2Vector = VECTOR3x(8, 9, 10);
	*outContactArea = 11;
	return true;
}

static void testGetHandler() {
	IntersectionManager * intersectionManager;
	IntersectionManager_collisionTestHandler handler;
	
	intersectionManager = IntersectionManager_create();
	intersectionManager->defaultCollisionTestHandler = (void *) 0x1;
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 0, 0);
	TestCase_assert(handler == (void *) 0x1, "Expected 0x1 but got %p", handler);
	intersectionManager->defaultCollisionTestHandler = NULL;
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 0, 0);
	TestCase_assert(handler == NULL, "Expected NULL but got %p", handler);
	
	IntersectionManager_setHandlersForTypePair(intersectionManager, 0, 0, testHandler1, NULL);
	IntersectionManager_setHandlersForTypePair(intersectionManager, 0, 1, testHandler2, NULL);
	IntersectionManager_setHandlersForTypePair(intersectionManager, 1, 1, testHandler1, NULL);
	
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 0, 0);
	TestCase_assert(handler == testHandler1, "Expected %p but got %p", testHandler1, handler);
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 0, 1);
	TestCase_assert(handler == testHandler2, "Expected %p but got %p", testHandler2, handler);
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 1, 1);
	TestCase_assert(handler == testHandler1, "Expected %p but got %p", testHandler1, handler);
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 1, 0);
	TestCase_assert(handler == NULL, "Expected NULL but got %p", handler);
	
	IntersectionManager_setHandlersForTypePair(intersectionManager, 0, 0, NULL, NULL);
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, 0, 0);
	TestCase_assert(handler == NULL, "Expected NULL but got %p", handler);
	
	IntersectionManager_dispose(intersectionManager);
}

#define verifyHandler(type1, type2, expectedHandler) \
	handler = IntersectionManager_getCollisionTestHandler(intersectionManager, type1, type2); \
	TestCase_assert(handler == expectedHandler, "Expected %p but got %p (line %d)", expectedHandler, handler, line);

static void verifyStandardHandlers(int line, IntersectionManager * intersectionManager) {
	IntersectionManager_collisionTestHandler handler;
	
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_RECT_2D, intersectionHandler_rect2D_rect2D)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_CIRCLE, intersectionHandler_rect2D_circle)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_LINE_2D, intersectionHandler_rect2D_line2D)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_POLYGON, intersectionHandler_rect2D_polygon)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_CIRCLE, intersectionHandler_circle_circle)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_LINE_2D, intersectionHandler_circle_line2D)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_POLYGON, intersectionHandler_circle_polygon)
	verifyHandler(COLLISION_SHAPE_LINE_2D, COLLISION_SHAPE_LINE_2D, intersectionHandler_line2D_line2D)
	verifyHandler(COLLISION_SHAPE_LINE_2D, COLLISION_SHAPE_POLYGON, intersectionHandler_line2D_polygon)
	verifyHandler(COLLISION_SHAPE_POLYGON, COLLISION_SHAPE_POLYGON, intersectionHandler_polygon_polygon)
	
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_BOX_3D, intersectionHandler_box3D_box3D)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_SPHERE, intersectionHandler_box3D_sphere)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_LINE_3D, intersectionHandler_box3D_line3D)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_CAPSULE, intersectionHandler_box3D_capsule)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_STATIC_TRIMESH, intersectionHandler_box3D_trimesh)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_SPHERE, intersectionHandler_sphere_sphere)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_LINE_3D, intersectionHandler_sphere_line3D)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_CAPSULE, intersectionHandler_sphere_capsule)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_STATIC_TRIMESH, intersectionHandler_sphere_trimesh)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_LINE_3D, intersectionHandler_line3D_line3D)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_CAPSULE, intersectionHandler_line3D_capsule)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_STATIC_TRIMESH, intersectionHandler_line3D_trimesh)
	verifyHandler(COLLISION_SHAPE_CAPSULE, COLLISION_SHAPE_CAPSULE, intersectionHandler_capsule_capsule)
	verifyHandler(COLLISION_SHAPE_CAPSULE, COLLISION_SHAPE_STATIC_TRIMESH, intersectionHandler_capsule_trimesh)
	verifyHandler(COLLISION_SHAPE_STATIC_TRIMESH, COLLISION_SHAPE_STATIC_TRIMESH, intersectionHandler_trimesh_trimesh)
}

static void verifyNoStandardHandlers(int line, IntersectionManager * intersectionManager) {
	IntersectionManager_collisionTestHandler handler;
	
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_RECT_2D, NULL)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_CIRCLE, NULL)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_LINE_2D, NULL)
	verifyHandler(COLLISION_SHAPE_RECT_2D, COLLISION_SHAPE_POLYGON, NULL)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_CIRCLE, NULL)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_LINE_2D, NULL)
	verifyHandler(COLLISION_SHAPE_CIRCLE, COLLISION_SHAPE_POLYGON, NULL)
	verifyHandler(COLLISION_SHAPE_LINE_2D, COLLISION_SHAPE_LINE_2D, NULL)
	verifyHandler(COLLISION_SHAPE_LINE_2D, COLLISION_SHAPE_POLYGON, NULL)
	verifyHandler(COLLISION_SHAPE_POLYGON, COLLISION_SHAPE_POLYGON, NULL)
	
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_BOX_3D, NULL)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_SPHERE, NULL)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_LINE_3D, NULL)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_CAPSULE, NULL)
	verifyHandler(COLLISION_SHAPE_BOX_3D, COLLISION_SHAPE_STATIC_TRIMESH, NULL)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_SPHERE, NULL)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_LINE_3D, NULL)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_CAPSULE, NULL)
	verifyHandler(COLLISION_SHAPE_SPHERE, COLLISION_SHAPE_STATIC_TRIMESH, NULL)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_LINE_3D, NULL)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_CAPSULE, NULL)
	verifyHandler(COLLISION_SHAPE_LINE_3D, COLLISION_SHAPE_STATIC_TRIMESH, NULL)
	verifyHandler(COLLISION_SHAPE_CAPSULE, COLLISION_SHAPE_CAPSULE, NULL)
	verifyHandler(COLLISION_SHAPE_CAPSULE, COLLISION_SHAPE_STATIC_TRIMESH, NULL)
	verifyHandler(COLLISION_SHAPE_STATIC_TRIMESH, COLLISION_SHAPE_STATIC_TRIMESH, NULL)
}
#undef verifyHandler

static void testInitWithStandardHandlers() {
	IntersectionManager intersectionManager, * intersectionManagerPtr;
	
	intersectionManagerPtr = IntersectionManager_create();
	verifyInit(__LINE__, intersectionManagerPtr);
	verifyNoStandardHandlers(__LINE__, intersectionManagerPtr);
	IntersectionManager_addStandardHandlers(intersectionManagerPtr);
	verifyStandardHandlers(__LINE__, intersectionManagerPtr);
	IntersectionManager_dispose(intersectionManagerPtr);
	
	stemobject_assign_vtable(intersectionManager, IntersectionManager);
	IntersectionManager_init(&intersectionManager);
	verifyInit(__LINE__, &intersectionManager);
	verifyNoStandardHandlers(__LINE__, &intersectionManager);
	IntersectionManager_dispose(&intersectionManager);
	
	stemobject_assign_vtable(intersectionManager, IntersectionManager);
	IntersectionManager_initWithStandardHandlers(&intersectionManager);
	verifyInit(__LINE__, &intersectionManager);
	verifyStandardHandlers(__LINE__, &intersectionManager);
	IntersectionManager_dispose(&intersectionManager);
	
	intersectionManagerPtr = IntersectionManager_create();
	verifyInit(__LINE__, intersectionManagerPtr);
	verifyNoStandardHandlers(__LINE__, intersectionManagerPtr);
	IntersectionManager_dispose(intersectionManagerPtr);
	
	intersectionManagerPtr = IntersectionManager_createWithStandardHandlers();
	verifyInit(__LINE__, intersectionManagerPtr);
	verifyStandardHandlers(__LINE__, intersectionManagerPtr);
	IntersectionManager_dispose(intersectionManagerPtr);
}

TEST_SUITE(IntersectionManagerTest,
           testInit,
           testGetHandler,
           testInitWithStandardHandlers)