#include "Collision.h"

#include "utils.h"
#include "../Game/Player.h"

namespace Collision
{

	TileCollisionRect::TileCollisionRect(const Point2f& pos, const Point2f& size, WorldTile* tile): pos(pos), size(size), tile(tile)
	{}
	bool PointVsRect(const Point2f p, const Collision::CollisionRect& r) {
		return ( p.x >= r.pos.x && p.y >= r.pos.y && p.x < r.pos.x + r.size.x && p.y < r.pos.y + r.size.y );
	}

	bool RectVsRect(const Collision::CollisionRect& r1, const Collision::CollisionRect r2) {
		return ( r1.pos.x < r2.pos.x + r2.size.x && r1.pos.x + r1.size.x > r2.pos.x && r1.pos.y < r2.pos.y + r2.size.y && r1.pos.y + r1.size.y > r2.pos.y );
	}

	bool RayVsRect(const Point2f& rayOrigin, const Point2f& rayDirection, const Collision::CollisionRect target, Point2f& contactPoint, Point2f& contactNormal,
		float& t_HitNear) {
		contactNormal = Point2f { 0, 0 };
		contactPoint = Point2f { 0, 0 };

		const Point2f inverseDirection = 1.0f / rayDirection;

		// Calculate intersections with rectangle bounding axes
		Point2f t_Near = Point2f{ target.pos.x - rayOrigin.x, target.pos.y - rayOrigin.y } * inverseDirection;
		Point2f t_Far =  Point2f{ target.pos.x + target.size.x - rayOrigin.x, target.pos.y + target.size.y - rayOrigin.y } * inverseDirection;

		if (std::isnan(t_Far.y) || std::isnan(t_Far.x))
			return false;
		if (std::isnan(t_Near.y) || std::isnan(t_Near.x))
			return false;

		// Sort distances
		if (t_Near.x > t_Far.x)
			std::swap(t_Near.x, t_Far.x);
		if (t_Near.y > t_Far.y)
			std::swap(t_Near.y, t_Far.y);

		// Early rejection		
		if (t_Near.x > t_Far.y || t_Near.y > t_Far.x)
			return false;

		// Closest 'time' will be the first contact
		t_HitNear = std::max(t_Near.x, t_Near.y);

		const float t_HitFar = std::min(t_Far.x, t_Far.y);

		// Reject if ray direction is pointing away from object
		if (t_HitFar < 0)
			return false;

		// Contact point of collision from parametric line equation
		contactPoint = rayOrigin + t_HitNear * rayDirection;

		if (t_Near.x > t_Near.y) {
			if (inverseDirection.x < 0) {
				contactNormal = Point2f { 1, 0 };
			}
			else {
				contactNormal = Point2f { -1, 0 };
			}
		}
		else if (t_Near.x < t_Near.y) {
			if (inverseDirection.y < 0) {
				contactNormal = Point2f { 0, 1 };
			}
			else {
				contactNormal = Point2f { 0, -1 };
			}
		}
		// If t_Near == t_Far, collision is diagonal so pointless

		return true;
	}

	bool DynamicRectVsRect(const Collision::CollisionRect& dynamicRectangle, float ElapsedTime, const Collision::CollisionRect& staticRectangle,
		Point2f& contactPoint, Point2f& contactNormal, float& contactTime) {
		// Check if dynamic rectangle is actually moving - we assume rectangles are NOT in collision to start
		if (dynamicRectangle.vel.x == 0 && dynamicRectangle.vel.y == 0) {
			return false;
		}
		
		// Expand target rectangle by source dimensions
		Collision::CollisionRect expandedTarget;
		expandedTarget.pos = Point2f{staticRectangle.pos.x - (dynamicRectangle.size / 2).x, staticRectangle.pos.y - (dynamicRectangle.size / 2).y};
		expandedTarget.size = staticRectangle.size + dynamicRectangle.size;
		
		Point2f RayOrigin = dynamicRectangle.pos + dynamicRectangle.size / 2;
		
		if (RayVsRect(RayOrigin, dynamicRectangle.vel * ElapsedTime, expandedTarget, contactPoint, contactNormal, contactTime)) {
			return ( contactTime >= 0.0f && contactTime < 1.0f );
		}
		else {
			return false;
		}
		return false;
	}

	bool ResolveDynamicRectVsRect(Collision::CollisionRect& dynamicRectangle, float ElapsedTime, Collision::CollisionRect* staticRectangle) {
		 Point2f contactPoint, contactNormal;
		 float contact_time = 0.0f;
		 if (DynamicRectVsRect(dynamicRectangle, ElapsedTime, *staticRectangle, contactPoint, contactNormal, contact_time)) {
		 	if (contactNormal.y > 0) {
		 		dynamicRectangle.ContactMap[CollisionDirection::Bottom] = staticRectangle;
		 	}
		 	if (contactNormal.x < 0) {
			 	dynamicRectangle.ContactMap[CollisionDirection::Left] = staticRectangle;
		 	}
		 	if (contactNormal.y < 0) {
			 	dynamicRectangle.ContactMap[CollisionDirection::Top] = staticRectangle;
		 	}
		 	if (contactNormal.x > 0) {
				dynamicRectangle.ContactMap[CollisionDirection::Right] = staticRectangle;
		 	}
		
		 	//dynamicRectangle.vel = dynamicRectangle.vel + contactNormal * Point2f(std::abs(dynamicRectangle.vel.x), std::abs(dynamicRectangle.vel.y)) * ( 1 - contact_time );
		 	dynamicRectangle.vel = dynamicRectangle.vel + contactNormal * -utils::DotProduct(dynamicRectangle.vel, contactNormal) * ( 1 - contact_time );
		 	return true;
		}
		return false;
	}

	bool ResolvePlayerVsRect(Player& player, float ElapsedTime, Collision::CollisionRect* staticRectangle) {
		CollisionRect rect = player.GetCollisionRect();
		Collision::ResolveDynamicRectVsRect(rect, ElapsedTime, staticRectangle);
		// std::map<CollisionDirection, TileCollisionRect*> test = rect.ContactMap;
		// player.SetContactMap(test);
		player.SetPosition(rect.pos);
		player.SetVelocity(rect.vel);
		return true;
	}
}