/*
  Copyright (c) 2011 Alex Diener
  
  This software is provided 'as-is', without any express or implied
  warranty. In no event will the authors be held liable for any damages
  arising from the use of this software.
  
  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:
  
  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
  
  Alex Diener adiener@sacredsoftware.net
*/

#include "CollisionSystem.h"
#include <stdbool.h>
#include <stdio.h>

#define X_AXIS 0
#define Y_AXIS 1

struct collision {
	float time;
	float surfaceArea;
	int axis;
};

// Arbitrarily small number used to account for floating point rounding error
#define COLLISION_EPSILON 0.00001f

#define min(lhs, rhs) ((lhs) < (rhs) ? (lhs) : (rhs))
#define max(lhs, rhs) ((lhs) > (rhs) ? (lhs) : (rhs))

static bool movingToStaticIntersectionTest(struct movingObject * movingObject, struct staticObject staticObject, struct collision * outCollision) {
	float timeX, timeY;
	bool collisionX = false, collisionY = false;
	float newX, newY;
	float staticLeft, staticRight, staticBottom, staticTop;
	float movingLeft, movingRight, movingBottom, movingTop;
	float lastMovingLeft, lastMovingRight, lastMovingBottom, lastMovingTop;
	
	staticLeft = staticObject.position.x;
	staticRight = staticLeft + staticObject.width;
	staticBottom = staticObject.position.y;
	staticTop = staticBottom + staticObject.height;
	movingLeft = movingObject->position.x;
	movingRight = movingLeft + movingObject->width;
	movingBottom = movingObject->position.y;
	movingTop = movingBottom + movingObject->height;
	lastMovingLeft = movingObject->lastPosition.x;
	lastMovingRight = lastMovingLeft + movingObject->width;
	lastMovingBottom = movingObject->lastPosition.y;
	lastMovingTop = lastMovingBottom + movingObject->height;
	
	// Incorrect sweeping, but better than nothing
	if (min(movingBottom, lastMovingBottom) < staticTop - COLLISION_EPSILON && max(movingTop, lastMovingTop) > staticBottom + COLLISION_EPSILON) {
		if (lastMovingRight <= staticLeft + COLLISION_EPSILON && movingRight > staticLeft + COLLISION_EPSILON) {
			collisionX = true;
			timeX = (staticLeft - lastMovingRight) / (movingLeft - lastMovingLeft);
			
		} else if (lastMovingLeft >= staticRight - COLLISION_EPSILON && movingLeft < staticRight - COLLISION_EPSILON) {
			collisionX = true;
			timeX = (staticRight - lastMovingLeft) / (movingLeft - lastMovingLeft);
		}
	}
	
	if (min(movingLeft, lastMovingLeft) < staticRight - COLLISION_EPSILON && max(movingRight, lastMovingRight) > staticLeft + COLLISION_EPSILON) {
		if (lastMovingTop <= staticBottom + COLLISION_EPSILON && movingTop > staticBottom + COLLISION_EPSILON) {
			collisionY = true;
			timeY = (staticBottom - lastMovingTop) / (movingBottom - lastMovingBottom);
			
		} else if (lastMovingBottom >= staticTop - COLLISION_EPSILON && movingBottom < staticTop - COLLISION_EPSILON) {
			collisionY = true;
			timeY = (staticTop - lastMovingBottom) / (movingBottom - lastMovingBottom);
		}
	}
	
	if (collisionX || collisionY) {
		if (!collisionX || (collisionY && timeY < timeX)) {
			outCollision->time = timeY;
			outCollision->axis = Y_AXIS;
			newX = lastMovingLeft + (movingLeft - lastMovingLeft) * timeY;
			outCollision->surfaceArea = min(staticRight, newX + movingObject->width) - max(staticLeft, newX);
			return true;
		}
		
		outCollision->time = timeX;
		outCollision->axis = X_AXIS;
		newY = lastMovingBottom + (movingBottom - lastMovingBottom) * timeX;
		outCollision->surfaceArea = min(staticTop, newY + movingObject->height) - max(staticBottom, newY);
		return true;
	}
	
	return false;
}

static bool movingToMovingIntersectionTest(struct movingObject * object1, struct movingObject * object2, struct collision * outCollision) {
	float timeX, timeY;
	bool collisionX = false, collisionY = false;
	float newX, newY;
	float left1, right1, bottom1, top1;
	float lastLeft1, lastRight1, lastBottom1, lastTop1;
	float left2, right2, bottom2, top2;
	float lastLeft2, lastRight2, lastBottom2, lastTop2;
	
	left1 = object1->position.x;
	right1 = left1 + object1->width;
	bottom1 = object1->position.y;
	top1 = bottom1 + object1->height;
	lastLeft1 = object1->lastPosition.x;
	lastRight1 = lastLeft1 + object1->width;
	lastBottom1 = object1->lastPosition.y;
	lastTop1 = lastBottom1 + object1->height;
	left2 = object2->position.x;
	right2 = left2 + object2->width;
	bottom2 = object2->position.y;
	top2 = bottom2 + object2->height;
	lastLeft2 = object2->lastPosition.x;
	lastRight2 = lastLeft2 + object2->width;
	lastBottom2 = object2->lastPosition.y;
	lastTop2 = lastBottom2 + object2->height;
	
	// Incorrect sweeping, but better than nothing
	if (min(bottom1, lastBottom1) < max(top2, lastTop2) - COLLISION_EPSILON && max(top1, lastTop1) > min(bottom2, lastBottom2) + COLLISION_EPSILON) {
		if (lastRight1 <= lastLeft2 + COLLISION_EPSILON && right1 > left2 + COLLISION_EPSILON) {
			collisionX = true;
			timeX = (lastLeft2 - lastRight1) / (left1 - lastLeft1);
			
		} else if (lastLeft1 >= lastRight2 - COLLISION_EPSILON && left1 < right2 - COLLISION_EPSILON) {
			collisionX = true;
			timeX = (lastRight2 - lastLeft1) / (left1 - lastLeft1);
		}
	}
	
	if (min(left1, lastLeft1) < max(right2, lastRight2) - COLLISION_EPSILON && max(right1, lastRight1) > min(left2, lastLeft2) + COLLISION_EPSILON) {
		if (lastTop1 <= lastBottom2 + COLLISION_EPSILON && top1 > bottom2 + COLLISION_EPSILON) {
			collisionY = true;
			timeY = (lastBottom2 - lastTop1) / (bottom1 - lastBottom1);
			
		} else if (lastBottom1 >= lastTop2 - COLLISION_EPSILON && bottom1 < top2 - COLLISION_EPSILON) {
			collisionY = true;
			timeY = (lastTop2 - lastBottom1) / (bottom1 - lastBottom1);
		}
	}
	
	if (collisionX || collisionY) {
		if (!collisionX || (collisionY && timeY < timeX)) {
			outCollision->time = timeY;
			outCollision->axis = Y_AXIS;
			// THIS IS WRONG (but doesn't matter much yet)
			newX = lastLeft1 + (left1 - lastLeft1) * timeY;
			outCollision->surfaceArea = min(right2, newX + object1->width) - max(left2, newX);
			return true;
		}
		
		outCollision->time = timeX;
		outCollision->axis = X_AXIS;
		// THIS IS WRONG (but doesn't matter much yet)
		newY = lastBottom1 + (bottom1 - lastBottom1) * timeX;
		outCollision->surfaceArea = min(top2, newY + object1->height) - max(bottom2, newY);
		return true;
	}
	
	return false;
}

static void resolveStaticCollision(struct movingObject * object, struct collision collision, float timesliceRemaining) {
	object->position.x = object->lastPosition.x + (object->position.x - object->lastPosition.x) * collision.time;
	object->position.y = object->lastPosition.y + (object->position.y - object->lastPosition.y) * collision.time;
	if (collision.axis == X_AXIS) {
		object->velocity.x = 0.0f;
	} else {
		object->velocity.y = 0.0f;
	}
	
	object->lastPosition = object->position;
	object->position.x += object->velocity.x * (1.0f - collision.time) * timesliceRemaining;
	object->position.y += object->velocity.y * (1.0f - collision.time) * timesliceRemaining;
}

static void resolveMovingCollision(struct movingObject * object1, struct movingObject * object2, struct collision collision, float timesliceRemaining) {
	object1->position.x = object1->lastPosition.x + (object1->position.x - object1->lastPosition.x) * collision.time;
	object1->position.y = object1->lastPosition.y + (object1->position.y - object1->lastPosition.y) * collision.time;
	object2->position.x = object2->lastPosition.x + (object2->position.x - object2->lastPosition.x) * collision.time;
	object2->position.y = object2->lastPosition.y + (object2->position.y - object2->lastPosition.y) * collision.time;
	
	if (collision.axis == X_AXIS) {
		object1->velocity.x = object2->velocity.x = (object1->velocity.x + object2->velocity.x) * 0.5f;
	} else {
		object1->velocity.y = object2->velocity.y = (object1->velocity.y + object2->velocity.y) * 0.5f;
	}
	
	object1->lastPosition = object1->position;
	object1->position.x += object1->velocity.x * (1.0f - collision.time) * timesliceRemaining;
	object1->position.y += object1->velocity.y * (1.0f - collision.time) * timesliceRemaining;
	object2->lastPosition = object2->position;
	object2->position.x += object2->velocity.x * (1.0f - collision.time) * timesliceRemaining;
	object2->position.y += object2->velocity.y * (1.0f - collision.time) * timesliceRemaining;
}

// Arbitrarily small number used to account for floating point rounding error
#define TIMESLICE_EPSILON 0.00001f

// Arbitrarily high number used prevent infinite loops in degenerate cases where collisions are unresolvable
#define MAX_TIMESLICE_SUBDIVISIONS 32

void resolveCollisions(struct movingObject * movingObjects, int numberOfMovingObjects, struct staticObject * staticObjects, int numberOfStaticObjects) {
	int movingObjectIndex, movingObjectIndex2, staticObjectIndex;
	struct movingObject * object, * object2;
	float timesliceRemaining;
	int numberOfTimesSubdivided;
	struct collision collision, bestCollision;
	struct movingObject * bestCollisionObject, * bestCollisionSecondaryObject;
	bool collided, bestCollisionStatic;
	
	timesliceRemaining = 1.0f;
	numberOfTimesSubdivided = 0;
	
	while (timesliceRemaining > TIMESLICE_EPSILON) {
		bestCollision.time = 1.0f + TIMESLICE_EPSILON;
		collided = false;
		
		for (movingObjectIndex = 0; movingObjectIndex < numberOfMovingObjects; movingObjectIndex++) {
			object = &movingObjects[movingObjectIndex];
			
			for (staticObjectIndex = 0; staticObjectIndex < numberOfStaticObjects; staticObjectIndex++) {
				if (movingToStaticIntersectionTest(object, staticObjects[staticObjectIndex], &collision)) {
					if (collision.time < bestCollision.time ||
					    (collision.time < bestCollision.time + TIMESLICE_EPSILON &&
					     collision.surfaceArea > bestCollision.surfaceArea)) {
						collided = true;
						bestCollision = collision;
						bestCollisionObject = object;
						bestCollisionStatic = true;
					}
				}
			}
			
			for (movingObjectIndex2 = movingObjectIndex + 1; movingObjectIndex2 < numberOfMovingObjects; movingObjectIndex2++) {
				object2 = &movingObjects[movingObjectIndex2];
				
				if (movingToMovingIntersectionTest(object, object2, &collision)) {
					if (collision.time < bestCollision.time ||
					    (collision.time < bestCollision.time + TIMESLICE_EPSILON &&
					     collision.surfaceArea > bestCollision.surfaceArea)) {
						collided = true;
						bestCollision = collision;
						bestCollisionObject = object;
						bestCollisionSecondaryObject = object2;
						bestCollisionStatic = false;
					}
				}
			}
		}
			
		if (!collided) {
			break;
		}
		
		if (bestCollisionStatic) {
			resolveStaticCollision(bestCollisionObject, bestCollision, timesliceRemaining);
		} else {
			resolveMovingCollision(bestCollisionObject, bestCollisionSecondaryObject, bestCollision, timesliceRemaining);
		}
		
		timesliceRemaining = (1.0f - bestCollision.time) * timesliceRemaining;
		numberOfTimesSubdivided++;
		
		if (numberOfTimesSubdivided > MAX_TIMESLICE_SUBDIVISIONS) {
			printf("Warning: Maximum timeslice subdivisons reached; collision resolution may be incomplete\n");
			break;
		}
	}
}