/*
  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);
		}
		
		// Compensate somewhat for incorrect sweeping by discarding collisions that don't intersect at collision time
		// Without this, collisions with cracks where you're moving perpendicular to the wall faster than parallel to it will be registered
		if (collisionX) {
			newY = lastMovingBottom + (movingBottom - lastMovingBottom) * timeX;
			if (newY >= staticTop - COLLISION_EPSILON || newY + staticObject.height <= staticBottom + COLLISION_EPSILON) {
				collisionX = false;
			}
		}
	}
	
	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) {
			newX = lastMovingLeft + (movingLeft - lastMovingLeft) * timeY;
			if (newX >= staticRight - COLLISION_EPSILON || newY + staticObject.width <= staticLeft + COLLISION_EPSILON) {
				collisionY = false;
			}
		}
	}
	
	if (collisionX || collisionY) {
		if (!collisionX || (collisionY && timeY < timeX)) {
			outCollision->time = timeY;
			outCollision->axis = Y_AXIS;
			outCollision->surfaceArea = min(staticRight, newX + movingObject->width) - max(staticLeft, newX);
			return true;
		}
		
		outCollision->time = timeX;
		outCollision->axis = X_AXIS;
		outCollision->surfaceArea = min(staticTop, newY + movingObject->height) - max(staticBottom, 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;
}

// 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 * movingObject, struct staticObject * staticObjects, int numberOfStaticObjects) {
	int staticObjectIndex;
	float timesliceRemaining;
	int numberOfTimesSubdivided;
	struct collision collision, bestCollision;
	bool collided;
	
	timesliceRemaining = 1.0f;
	numberOfTimesSubdivided = 0;
	
	while (timesliceRemaining > TIMESLICE_EPSILON) {
		bestCollision.time = 1.0f + TIMESLICE_EPSILON;
		collided = false;
		
		for (staticObjectIndex = 0; staticObjectIndex < numberOfStaticObjects; staticObjectIndex++) {
			if (movingToStaticIntersectionTest(movingObject, staticObjects[staticObjectIndex], &collision)) {
				if (collision.time < bestCollision.time ||
						(collision.time < bestCollision.time + TIMESLICE_EPSILON &&
						 collision.surfaceArea > bestCollision.surfaceArea)) {
					collided = true;
					bestCollision = collision;
				}
			}
		}
		
		if (!collided) {
			break;
		}
		
		resolveStaticCollision(movingObject, bestCollision, timesliceRemaining);
		
		timesliceRemaining = (1.0f - bestCollision.time) * timesliceRemaining;
		numberOfTimesSubdivided++;
		
		if (numberOfTimesSubdivided > MAX_TIMESLICE_SUBDIVISIONS) {
			fprintf(stderr, "Warning: Maximum timeslice subdivisons reached; collision resolution may be incomplete\n");
			break;
		}
	}
}