#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));
}


physics_t physics_default() {
	return (physics_t) {
		.type=COLLIDERTYPE_NONE,
		.velocity_x = 0.f,
		.velocity_y = 0.f,
		.solver = &solve_collision_slide
	};
}

void object_broadcast_collision(object_t* this, object_t* other) {
	if(this->physics.evt_collision != NULL) {
		this->physics.evt_collision(this, other);
	}
}
short can_collide(const object_t* this) {
	return object_is_valid(this) && 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->physics.aabb.x + a->sprite.x, aminy = a->physics.aabb.y + a->sprite.x;
	const float amaxx = aminx + a->physics.aabb.w, amaxy = aminy + a->physics.aabb.h;
	const float bminx = b->physics.aabb.x, bminy = b->physics.aabb.y;
	const float bmaxx = b->physics.aabb.x + b->physics.aabb.w, bmaxy = b->physics.aabb.y + b->physics.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->physics.circle.x, ay = a->sprite.y + a->physics.circle.y,
		  		bx = b->sprite.x + b->physics.circle.x, by = b->sprite.y + b->physics.circle.y;
	const float dx = fabsf(ax-bx), dy = fabsf(ay-by);
	const float sqrdist = dx*dx+dy*dy;
	const float mindist = a->physics.circle.radius + b->physics.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->physics.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->physics.aabb.w,
				bbminy = aabb->physics.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->physics.aabb.h;
	const float cx = circle->sprite.x + circle->physics.circle.x,
				cy = circle->sprite.y + circle->physics.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->physics.circle.radius*circle->physics.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->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_AABB) {
		return _collision_aabb_aabb(a, b);
	} else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_CIRCLE) {
		return _collision_circle_circle(a, b);
	} else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_AABB) {
		return _collision_circle_aabb(a, b);
	} else if(a->physics.type == COLLIDERTYPE_AABB && b->physics.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->physics.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->physics.aabb.w,
	            bbminy = aabb->physics.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->physics.aabb.h;
	// the centre of the circle in world space
	const float cx = circle->sprite.x + circle->physics.circle.x,
	            cy = circle->sprite.y + circle->physics.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->physics.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->physics.circle.x + a->sprite.x, y1 = a->physics.circle.y + a->sprite.y;
	const float x2 = b->physics.circle.x + b->sprite.x, y2 = b->physics.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->physics.circle.radius + b->physics.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->physics.aabb.x + a->physics.aabb.w + a->sprite.x) - (b->physics.aabb.x + b->sprite.x);
	float left = (a->physics.aabb.x + a->sprite.x) - (b->physics.aabb.x + b->physics.aabb.w + b->sprite.x);
	float top = (a->physics.aabb.y + a->sprite.y) - (b->physics.aabb.y + b->physics.aabb.w + b->sprite.y);
	float bottom = (a->physics.aabb.y + a->physics.aabb.h) - (b->physics.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->physics.type == COLLIDERTYPE_AABB && b->physics.type == COLLIDERTYPE_AABB) {
		return _solve_aabb_aabb(a, b, out_px, out_py);
	} else if(a->physics.type == COLLIDERTYPE_AABB && b->physics.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->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_AABB) {
		return _solve_circle_aabb(a, b, out_px, out_py);
	} else if(a->physics.type == COLLIDERTYPE_CIRCLE && b->physics.type == COLLIDERTYPE_CIRCLE) {
		return _solve_circle_circle(a, b, out_px, out_py);
	}
}

void solve_collision_slide(object_t* left, object_t* right) {
	float dx, dy;
	const float d = get_solve_force(left, right, &dx, &dy);
	left->sprite.x += dx;
	left->sprite.y += dy;
}

static inline
void _solve_move(object_t* this) {
	// 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 = world_get_object(i);
		// check collision, return if found
		if(can_collide(other) && this != other && _collision_check(other, this)) {
			object_broadcast_collision(other, this);
			object_broadcast_collision(this, other);
			this->physics.solver(this, other);
		}
	}
}

void physics_move(object_t* this, float delta_time) {
	const float max_step_size = this->physics.max_interpolate_step_size;
	// calculate step delta
	float dx = this->physics.velocity_x * delta_time, dy = this->physics.velocity_y * delta_time;
	const float target_x = this->sprite.x + dx,
	            target_y = this->sprite.y + dy;
	if(dx == 0 && dy == 0)
		return;

	// calculate direction x,y
	float m = sqrtf(dx*dx + dy*dy);
	dx = dx / m * max_step_size;
	dy = dy / m * max_step_size;

	const 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(this)) {
		this->sprite.x = target_x;
		this->sprite.y = target_y;
		return;
	}

	if(step_count == 0) {
		this->sprite.x = target_x;
		this->sprite.y = target_y;
		_solve_move(this);
		return;
	}

	/*
	 * 1. move towards target
	 * 2. check collision with every other object
	 */
	for(int steps = 0; steps <= step_count && (this->sprite.x != target_x || this->sprite.y != target_y); ++steps) {
		// move towards target, snap to target if distance is too low
		const float distx = fabsf(this->sprite.x - target_x), disty = fabsf(this->sprite.y - target_y);
		const float sqdist = distx*distx + disty*disty;
		if(sqdist > max_step_size*max_step_size) {
			this->sprite.x += dx;
			this->sprite.y += dy;
		} else {
			this->sprite.x = target_x;
			this->sprite.y = target_y;
		}

		_solve_move(this);
	}
}