#include "collision/CollisionPairQueue.h"
#include "unittest/TestSuite.h"
#include "utilities/printfFormats.h"

static void verifyInit(CollisionPairQueue * queue, int line) {
	TestCase_assert(queue != NULL, "Expected non-NULL but got NULL (line %d)", line);
	TestCase_assert(queue->vtable->dispose == CollisionPairQueue_dispose, "Expected %p but got %p (line %d)", CollisionPairQueue_dispose, queue->vtable->dispose, line);
	TestCase_assert(queue->pairCount == 0, "Expected 0 but got " SIZE_T_FORMAT " (line %d)", queue->pairCount, line);
	TestCase_assert(queue->nextPairCount == 0, "Expected 0 but got " SIZE_T_FORMAT " (line %d)", queue->nextPairCount, line);
	TestCase_assert(queue->nextObjectCount == 0, "Expected 0 but got " SIZE_T_FORMAT " (line %d)", queue->nextObjectCount, line);
	TestCase_assert(queue->queuePosition == 0, "Expected 0 but got " SIZE_T_FORMAT " (line %d)", queue->queuePosition, line);
}

static void testInit() {
	CollisionPairQueue queue, * queuePtr;
	
	memset(&queue, 0x00, sizeof(queue));
	stemobject_assign_vtable(queue, CollisionPairQueue);
	CollisionPairQueue_init(&queue);
	verifyInit(&queue, __LINE__);
	CollisionPairQueue_dispose(&queue);
	
	memset(&queue, 0xFF, sizeof(queue));
	stemobject_assign_vtable(queue, CollisionPairQueue);
	CollisionPairQueue_init(&queue);
	verifyInit(&queue, __LINE__);
	CollisionPairQueue_dispose(&queue);
	
	queuePtr = CollisionPairQueue_create();
	verifyInit(queuePtr, __LINE__);
	CollisionPairQueue_dispose(queuePtr);
}

static void testAddInitialPairs() {
	CollisionPairQueue * queue;
	CollisionObject objectsSequential[3];
	CollisionObject * objects[3];
	CollisionObject * object1, * object2;
	bool success;
	
	queue = CollisionPairQueue_create();
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(!success, "Expected false but got true");
	
	stemobject_assign_vtable(objectsSequential[0], CollisionObject);
	CollisionObject_init(&objectsSequential[0], NULL, NULL, NULL);
	stemobject_assign_vtable(objectsSequential[1], CollisionObject);
	CollisionObject_init(&objectsSequential[1], NULL, NULL, NULL);
	stemobject_assign_vtable(objectsSequential[2], CollisionObject);
	CollisionObject_init(&objectsSequential[2], NULL, NULL, NULL);
	objects[0] = &objectsSequential[0];
	objects[1] = &objectsSequential[1];
	objects[2] = &objectsSequential[2];
	CollisionPairQueue_addInitialPairs(queue, objects, 3, NULL, 0);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[0], "Expected %p but got %p", objects[0], object1);
	TestCase_assert(object2 == objects[1], "Expected %p but got %p", objects[1], object2);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[0], "Expected %p but got %p", objects[0], object1);
	TestCase_assert(object2 == objects[2], "Expected %p but got %p", objects[2], object2);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[1], "Expected %p but got %p", objects[1], object1);
	TestCase_assert(object2 == objects[2], "Expected %p but got %p", objects[2], object2);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(!success, "Expected false but got true");
	
	CollisionObject_dispose(objects[0]);
	CollisionObject_dispose(objects[1]);
	CollisionObject_dispose(objects[2]);
	CollisionPairQueue_dispose(queue);
}

static void testAddNextPair() {
	CollisionPairQueue * queue;
	CollisionObject objectsSequential[3];
	CollisionObject * objects[3];
	CollisionObject * object1, * object2;
	bool success;
	
	queue = CollisionPairQueue_create();
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(!success, "Expected false but got true");
	
	stemobject_assign_vtable(objectsSequential[0], CollisionObject);
	CollisionObject_init(&objectsSequential[0], NULL, NULL, NULL);
	stemobject_assign_vtable(objectsSequential[1], CollisionObject);
	CollisionObject_init(&objectsSequential[1], NULL, NULL, NULL);
	stemobject_assign_vtable(objectsSequential[2], CollisionObject);
	CollisionObject_init(&objectsSequential[2], NULL, NULL, NULL);
	objects[0] = &objectsSequential[0];
	objects[1] = &objectsSequential[1];
	objects[2] = &objectsSequential[2];
	
	CollisionPairQueue_addNextPair(queue, objects[0], objects[1]);
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(!success, "Expected false but got true");
	success = CollisionPairQueue_nextIteration(queue, NULL, 0, NULL, 0);
	TestCase_assert(success, "Expected true but got false");
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[0], "Expected %p but got %p", objects[0], object1);
	TestCase_assert(object2 == objects[1], "Expected %p but got %p", objects[1], object2);
	
	CollisionPairQueue_addNextPair(queue, objects[0], objects[2]);
	CollisionPairQueue_addNextPair(queue, objects[1], objects[2]);
	CollisionPairQueue_nextIteration(queue, NULL, 0, NULL, 0);
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[0], "Expected %p but got %p", objects[0], object1);
	TestCase_assert(object2 == objects[2], "Expected %p but got %p", objects[2], object2);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(success, "Expected true but got false");
	TestCase_assert(object1 == objects[1], "Expected %p but got %p", objects[1], object1);
	TestCase_assert(object2 == objects[2], "Expected %p but got %p", objects[2], object2);
	
	success = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assert(!success, "Expected false but got true");
	
	CollisionObject_dispose(objects[0]);
	CollisionObject_dispose(objects[1]);
	CollisionObject_dispose(objects[2]);
	CollisionPairQueue_dispose(queue);
}

static void testAddNextObject() {
	CollisionPairQueue * queue = CollisionPairQueue_create();
	CollisionObject * object1, * object2;
	bool hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolFalse(hasPair);
	
	CollisionObject collisionObjects[3];
	stemobject_assign_vtable(collisionObjects[0], CollisionObject);
	CollisionObject_init(&collisionObjects[0], NULL, NULL, NULL);
	stemobject_assign_vtable(collisionObjects[1], CollisionObject);
	CollisionObject_init(&collisionObjects[1], NULL, NULL, NULL);
	stemobject_assign_vtable(collisionObjects[2], CollisionObject);
	CollisionObject_init(&collisionObjects[2], NULL, NULL, NULL);
	CollisionObject * objects[3] = {&collisionObjects[0], &collisionObjects[1], &collisionObjects[2]};
	
	hasPair = CollisionPairQueue_nextIteration(queue, objects, 3, NULL, 0);
	TestCase_assertBoolFalse(hasPair);
	
	CollisionPairQueue_addNextObject(queue, objects[0]);
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolFalse(hasPair);
	
	hasPair = CollisionPairQueue_nextIteration(queue, objects, 3, NULL, 0);
	TestCase_assertBoolTrue(hasPair);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolTrue(hasPair);
	TestCase_assertPointerEqual(object1, objects[0]);
	TestCase_assertPointerEqual(object2, objects[1]);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolTrue(hasPair);
	TestCase_assertPointerEqual(object1, objects[0]);
	TestCase_assertPointerEqual(object2, objects[2]);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolFalse(hasPair);
	
	CollisionPairQueue_addNextObject(queue, objects[1]);
	CollisionPairQueue_addNextObject(queue, objects[1]);
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolFalse(hasPair);
	
	hasPair = CollisionPairQueue_nextIteration(queue, objects, 3, NULL, 0);
	TestCase_assertBoolTrue(hasPair);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolTrue(hasPair);
	TestCase_assertPointerEqual(object1, objects[1]);
	TestCase_assertPointerEqual(object2, objects[0]);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolTrue(hasPair);
	TestCase_assertPointerEqual(object1, objects[1]);
	TestCase_assertPointerEqual(object2, objects[2]);
	
	hasPair = CollisionPairQueue_nextPair(queue, &object1, &object2);
	TestCase_assertBoolFalse(hasPair);
	
	hasPair = CollisionPairQueue_nextIteration(queue, objects, 3, NULL, 0);
	TestCase_assertBoolFalse(hasPair);
	
	CollisionObject_dispose(objects[0]);
	CollisionObject_dispose(objects[1]);
	CollisionObject_dispose(objects[2]);
	CollisionPairQueue_dispose(queue);
}

TEST_SUITE(CollisionPairQueueTest,
           testInit,
           testAddInitialPairs,
           testAddNextPair,
           testAddNextObject)