#include "physics.h"
#include "object.h"
#include "world.h"
#include "math/vec.h"

static inline
float fclampf(float x, float min_, float max_) {
	return fminf(max_, fmaxf(min_, x));
}


collider_t collider_default() {
	return (collider_t) {
		.type=COLLIDERTYPE_NONE
	};
}

void object_broadcast_collision(object_t* this, object_t* other) {
	if(this->collider.evt_collision != NULL) {
		this->collider.evt_collision(this, other);
	}
}
short can_collide(const object_t* this) {
	return this->active && this->enabled;
}

static inline
int _rect_overlap(float aminx, float aminy, float amaxx, float amaxy, float bminx, float bminy, float bmaxx, float bmaxy) {
	return
	(
		(aminx < bmaxx && aminx > bmaxx) 
		||
		(amaxx > bminx && amaxx < bmaxx)
	) && (
		(aminy < bmaxy && aminy > bmaxy)
		||
		(amaxy > bminy && amaxy < bmaxy)
	);
}

static inline
short _collision_aabb_aabb(const object_t* a, const object_t* b) {
	const float aminx = a->collider.aabb.x + a->sprite.x, aminy = a->collider.aabb.y + a->sprite.x;
	const float amaxx = aminx + a->collider.aabb.w, amaxy = aminy + a->collider.aabb.h;
	const float bminx = b->collider.aabb.x, bminy = b->collider.aabb.y;
	const float bmaxx = b->collider.aabb.x + b->collider.aabb.w, bmaxy = b->collider.aabb.y + b->collider.aabb.h;

	return _rect_overlap(aminx, aminy, amaxx, amaxy, bminx, bminy, bmaxx, bmaxy);
}

static inline
short _collision_circle_circle(const object_t* a, const object_t* b) {
	const float ax = a->sprite.x + a->collider.circle.x, ay = a->sprite.y + a->collider.circle.y,
		  		bx = b->sprite.x + b->collider.circle.x, by = b->sprite.y + b->collider.circle.y;
	const float dx = fabsf(ax-bx), dy = fabsf(ay-by);
	const float sqrdist = dx*dx+dy*dy;
	const float mindist = a->collider.circle.radius + b->collider.circle.radius;
	const float mindistsqr = mindist*mindist;
	return sqrdist < mindistsqr;
}

static inline
short _collision_circle_aabb(const object_t* circle, const object_t* aabb) {
	// generate a point on the edge of the rectangle that is closest to the circle
	const float bbminx = aabb->collider.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->collider.aabb.w,
				bbminy = aabb->collider.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->collider.aabb.h;
	const float cx = circle->sprite.x + circle->collider.circle.x,
				cy = circle->sprite.y + circle->collider.circle.y;
	const float x = fclampf(cx, bbminx, bbmaxx),
				y = fclampf(cy, bbminy, bbmaxy);
	const float dx = fabsf(cx - x), dy = fabsf(cy - y);

	// calculate the square distance from the centre of the circle to the edge of the aabb
	const float distsqr = dx*dx+dy*dy;
	const float rsqr = circle->collider.circle.radius*circle->collider.circle.radius;

	// return if the square distance is larger than the square of the radius
	return distsqr < rsqr;
}

static inline
short _collision_check(const object_t* a, const object_t* b) {
	if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_AABB) {
		return _collision_aabb_aabb(a, b);
	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_CIRCLE) {
		return _collision_circle_circle(a, b);
	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_AABB) {
		return _collision_circle_aabb(a, b);
	} else if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_CIRCLE) {
		return _collision_circle_aabb(b, a);
	}
	return 0;
}

static inline
float _solve_circle_aabb(const object_t* circle, const object_t* aabb, float* out_px, float* out_py) {
	// generate a point on the edge of the rectangle that is closest to the circle
	const float bbminx = aabb->collider.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->collider.aabb.w,
	            bbminy = aabb->collider.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->collider.aabb.h;
	// the centre of the circle in world space
	const float cx = circle->sprite.x + circle->collider.circle.x,
	            cy = circle->sprite.y + circle->collider.circle.y;
	// the point on the rectangle closest to the centre of the circle
	const float x = fclampf(cx, bbminx, bbmaxx),
	            y = fclampf(cy, bbminy, bbmaxy);
	// the relative position of the point on the rectangle 
	const float dif_x = cx - x,
	            dif_y = cy - y;
	// absolute difference for use in calculating euclidean distance 
	const float dist_x = fabsf(dif_x),
	            dist_y = fabsf(dif_y);
	// euclidean distance
	const float dist = sqrt(dist_x*dist_x + dist_y*dist_y);
	const float solve_distance = circle->collider.circle.radius - dist;
	// distance to solve collision
	float solve_x, solve_y;
	normalize(dif_x, dif_y, &solve_x, &solve_y);
	*out_px = solve_x * solve_distance;
	*out_py = solve_y * solve_distance;
	return solve_distance;
}

static inline
float _solve_circle_circle(const object_t* a, const object_t* b, float* out_px, float* out_py) {
	const float x1 = a->collider.circle.x + a->sprite.x, y1 = a->collider.circle.y + a->sprite.y;
	const float x2 = b->collider.circle.x + b->sprite.x, y2 = b->collider.circle.y + b->sprite.y;
	const float dif_x = x1 - x2, dif_y = y1 - y2;
	const float difference = sqrtf(fabsf(dif_x*dif_x) + fabsf(dif_y*dif_y));
	const float target_difference = a->collider.circle.radius + b->collider.circle.radius;
	float dir_x, dir_y;
	normalize(dif_x, dif_y, &dir_x, &dir_y);
	*out_px = dir_x * target_difference;
	*out_py = dir_y * target_difference;
	return target_difference;
}

static inline
float _solve_aabb_aabb(const object_t* a, const object_t* b, float* out_px, float* out_py) {
	float right = (a->collider.aabb.x + a->collider.aabb.w + a->sprite.x) - (b->collider.aabb.x + b->sprite.x);
	float left = (a->collider.aabb.x + a->sprite.x) - (b->collider.aabb.x + b->collider.aabb.w + b->sprite.x);
	float top = (a->collider.aabb.y + a->sprite.y) - (b->collider.aabb.y + b->collider.aabb.w + b->sprite.y);
	float bottom = (a->collider.aabb.y + a->collider.aabb.h) - (b->collider.aabb.y + b->sprite.y);

	float ret = right;
	*out_px = right;
	*out_py = 0.f;
	if(fabsf(left) < fabsf(ret)) {
		*out_px = left;
		*out_py = 0.f;
		ret = left;
	}
	if(fabsf(top) < fabsf(ret)) {
		*out_px = 0.f;
		*out_py = top;
		ret = top;
	}

	if(fabsf(bottom) < fabsf(ret)) {
		*out_px = 0.f;
		*out_py = bottom;
		return bottom;
	}

	return ret;
}

float get_solve_force(const object_t* a, const object_t* b, float* out_px, float* out_py) {
	if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_AABB) {
		return _solve_aabb_aabb(a, b, out_px, out_py);
	} else if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_CIRCLE) {
		float penetration_distance = _solve_circle_aabb(b, a, out_px, out_py);
		*out_px = -(*out_px);
		*out_py = -(*out_py);
	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_AABB) {
		return _solve_circle_aabb(a, b, out_px, out_py);
	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_CIRCLE) {
		return _solve_circle_circle(a, b, out_px, out_py);
	}
}

static inline
void _solve_collision_slide(object_t* this, object_t* other, float old_x, float old_y, float new_x, float new_y) {
	float dx, dy;
	const float d = get_solve_force(this, other, &dx, &dy);
	this->sprite.x += dx;
	this->sprite.y += dy;
	
	return;
	this->sprite.x = old_x;
	this->sprite.y = new_y;
	if(!_collision_check(other, this)) {
		return;
	}

	this->sprite.x = new_x;
	this->sprite.y = old_y;
	if(!_collision_check(other, this)) {
		return;
	}

	this->sprite.x = old_x;
	this->sprite.y = old_y;
	if(!_collision_check(other, this)) {
		return;
	}
}

void interpolate_move(object_t* object, const float target_x, const float target_y, const float max_step_size, const int slide) {
	// calculate step delta
	float dx = target_x - object->sprite.x, dy = target_y - object->sprite.y;
	if(dx == 0 && dy == 0)
		return;
	// calculate direction x,y
	float m = sqrtf(dx*dx + dy*dy);
	if(dx != 0)
		dx /= m;
	if(dy != 0)
		dy /= m;
	dx *= max_step_size; dy *= max_step_size;
	int step_count = max_step_size / m;

	// ensure this object would ever collide
	// if it wouldn't collide anyway, just set position
	if(!can_collide(object)) {
		object->sprite.x = target_x;
		object->sprite.y = target_y;
		return;
	}

	/*
	 * 1. move towards target
	 * 2. check collision with every other object
	 */
	for(int steps = 0; steps < step_count && (object->sprite.x != target_x || object->sprite.y != target_y); ++steps) {
		// move towards target, snap to target if distance is too low
		const float old_x = object->sprite.x, old_y = object->sprite.y;
		float new_x, new_y;

		const float distx = fabsf(object->sprite.x - target_x), disty = fabsf(object->sprite.y - target_y);
		const float sqdist = distx*distx + disty*disty;
		if(sqdist > max_step_size) {
			object->sprite.x += dx;
			object->sprite.y += dy;
			new_x = object->sprite.x;
			new_y = object->sprite.y;
		} else {
			new_x = object->sprite.x = target_x;
			new_y = object->sprite.y = target_y;
		}

		// loop over all objects and check collision if applicable
		for(int i = 0; i < WORLD_NUM_OBJECTS; ++i) {
			// get pointer to other object
			object_t* other = g_objects + i;
			// check collision, return if found
			if(can_collide(other) && object != other && _collision_check(other, object)) {
				object_broadcast_collision(other, object);
				object_broadcast_collision(object, other);
				if(slide) {
					_solve_collision_slide(object, other, old_x, old_y, new_x, new_y);
				} else {
					object->sprite.x = old_x;
					object->sprite.y = old_y;
					return;
				}
			}
		}
	}
}