#include "Collision_Manager.h"
#include "Global_Properties.h"
#include "Time.h"

// Singleton implimentation
CollisionManager* CollisionManager::sInstance = 0;
CollisionManager& CollisionManager::I()
{
	if (sInstance == 0)
		sInstance = new CollisionManager();

	return *sInstance;
}

#ifdef USE_OCTREE
CollisionManager::CollisionManager() : 
	mOctree(glm::vec3(-GlobalProperties::ROOM_SIZE - 2.0f, -GlobalProperties::ROOM_SIZE - 2.0f, -GlobalProperties::ROOM_SIZE - 2.0f),
			glm::vec3(GlobalProperties::ROOM_SIZE + 2.0f, GlobalProperties::ROOM_SIZE + 2.0f, GlobalProperties::ROOM_SIZE + 2.0f),
			1) {}
#endif

void CollisionManager::CheckForCollisionsAll()
{
#ifdef USE_OCTREE

	std::vector<CollliderPair> pairs;
	mOctree.PotentialCollisions(pairs);

	for (unsigned i = 0; i < pairs.size(); ++i)
		CheckCollision(*pairs[i].a->GetPhysicsObject(), *pairs[i].b->GetPhysicsObject());

#endif

#ifndef USE_OCTREE

	for (int iterations = 0; iterations < GlobalProperties::NUM_OF_COLLISION_CHECKS; ++iterations)
	{
		std::list<Geo::GeoObjPhysics*>::iterator a;
		std::list<Geo::GeoObjPhysics*>::iterator b;
		int i = 0;
		for (a = mPhysicsObjects.begin(); i < mPhysicsObjects.size() - 1; ++i, ++a)
		{
			int j = i + 1;
			for (b = a; j < mPhysicsObjects.size(); ++j, ++b)
			{
				if (j == i + 1) // First time only
					++b;

				CheckCollision((**a), (**b));
			}
		}
	}

#endif

	/* This is the basic loop, it got a lot uglier when adding the list iterators
	for (int i = 0; i < 5 - 1; i++)
    {
        for (int j = i + 1; j < 5; j++)
        {
			std::cout << i << " " << j << std::endl;
        }
    }
	*/
}

void CollisionManager::CheckCollision(Geo::GeoObjPhysics& a, Geo::GeoObjPhysics& b)
{
	// Do not check for collisions if...
	if (a.mIsKinematic && b.mIsKinematic // If both are kinematic
	 || a.mIsSleeping && b.mIsSleeping	 // If both are sleeping
	 || !a.mCollider.IsEnabled() || !b.mCollider.IsEnabled()) //If one's colliders is disabled
	{
		return;
	}

	// Quickly check if objects are moving toward eachother
	if (a.mVelocity != glm::vec3(0) && b.mVelocity != glm::vec3(0) 
	 && glm::dot(b.GetPosition() - a.GetPosition(), a.mVelocity - b.mVelocity) < 0)
	{
		return;
	}


	bool hit = false;

	if (CollidersIntersect(a.mCollider, b.mCollider))
	{
		if (a.mIsKinematic || b.mIsKinematic) // If one is kinematic, then always make that one the second argument in the function
		{
			if (a.mIsKinematic)
				HandleCollision(b, a);
			else
				HandleCollision(a, b);
		}
		else
			HandleCollision(a, b);

		hit = true;
		a.ColliderHit();
		b.ColliderHit();
	}

	// If it already collided, just keep the colliding color
	/*
	if (!a.mCollider.IsEnabled())
		a.mCollider.SetHit(hit);
	if (!b.mCollider.IsEnabled())
		b.mCollider.SetHit(hit);
	*/
}

bool CollisionManager::CollidersIntersect(Collider& a, Collider& b)
{
	glm::vec3 aMax, aMin, bMax, bMin;
	aMax = a.GetBoundsMaxFromCenter();
	aMin = a.GetBoundsMinFromCenter();
	bMax = b.GetBoundsMaxFromCenter();
	bMin = b.GetBoundsMinFromCenter();

	if ((aMax.x >= bMin.x && aMin.x <= bMax.x)
    &&  (aMax.y >= bMin.y && aMin.y <= bMax.y)
    &&  (aMax.z >= bMin.z && aMin.z <= bMax.z))
		return true;
	else
		return false;
}

void CollisionManager::HandleCollision(Geo::GeoObjPhysics& a, Geo::GeoObjPhysics& b)
{
	bool checkAgain = false;

	/* Check if one of the objects were sleeping and should now be woken up */
	// Thanks to this site for giving me ideas for sleeping http://www.xbdev.net/physics/RigidBodyImpulseCubes/
	CheckWake(a, b);
	CheckWake(b, a);

	/* Test if a sleepable object should go to sleep */
	CheckSleep(a, b);
	CheckSleep(b, a);

	glm::vec3 newA = glm::vec3(0);
	glm::vec3 newB = glm::vec3(0);

	/* Calculate new velocities */
	if (!a.mIsSleeping) newA = (a.mVelocity * (a.mMass - b.mMass) + (2 * b.mMass * b.mVelocity)) / (a.mMass + b.mMass);
	if (!b.mIsKinematic && !b.mIsSleeping)
						newB = (b.mVelocity * (b.mMass - a.mMass) + (2 * a.mMass * a.mVelocity)) / (a.mMass + b.mMass);

	/* Use separating axis theorem to find normal and penetration */
	float penetration = 0;
	glm::vec3 normal = glm::vec3(0);
	SAT(a, b, penetration, normal);

	/* Use normal and penetration distance to move objects out of each other */
	const float k_slop = 0.01f; // Penetration allowance
	const float percent = 1.0f; // Penetration percentage to correct
	glm::vec3 correction = (std::max(penetration - k_slop, 0.0f) / (a.GetInverseMass() + b.GetInverseMass())) * percent * normal;

	if (a.mIsSleeping || b.mIsSleeping) // Collided with a sleeping object and didn't wake it: bounce off
	{
		if (a.mIsSleeping && !b.mIsKinematic)
			b.Translate(normal*penetration);
		else
			a.Translate(-normal*penetration);
	}
	else if (b.mIsKinematic) // Collided with kinematic object: bounce off
		a.Translate(-normal*penetration);
	else // Regular objects colliding: both push out
	{
		a.Translate(-a.GetInverseMass() * correction);
		b.Translate(b.GetInverseMass() * correction);
	}

	/* Apply new velocities */
	a.mVelocity = newA;
	if (!b.mIsKinematic)
		b.mVelocity = newB;

	CheckReflect(a, b, normal);
	CheckReflect(b, a, normal);

	/* Add some hacky friction */
	a.mVelocity *= 1 - GlobalProperties::FRICTION;
	if (!b.mIsKinematic) b.mVelocity *= 1 - GlobalProperties::FRICTION;
}

void CollisionManager::CheckSleep(Geo::GeoObjPhysics& aObjToCheck, Geo::GeoObjPhysics& aOther)	
{
	if (aObjToCheck.mAbleToSleep && !aObjToCheck.mIsSleeping)
	{
		if (aOther.mIsSleeping || aOther.mMass == 0)
		{
			aObjToCheck.Sleep();

			if (!aOther.mIsKinematic) // Floors/walls don't wake up anyway, don't want them to have a huge list and do nothing with it
				aOther.mObjectsSleepingOnThis.push_back(&aObjToCheck); // Add the now sleeping object to the object that it slept on's list
		}
	}
}

void CollisionManager::CheckWake(Geo::GeoObjPhysics& aObjToCheck, Geo::GeoObjPhysics& aOther)
{
	if (aObjToCheck.mIsSleeping)
	{
		float motion = glm::dot(aOther.mVelocity, aOther.mVelocity);
		if (motion > 20*GlobalProperties::SLEEP_EPSILON)
			aObjToCheck.WakeUp();
	}
}

void CollisionManager::CheckReflect(Geo::GeoObjPhysics& aObjToCheck, Geo::GeoObjPhysics& aOther, glm::vec3 aCollisionNorm)
{
	if (aOther.mMass == 0 || aOther.mIsSleeping) // Want it to bounce off of objects with infinite(0) mass
	{
		glm::vec3 reflect = 2.0f * aCollisionNorm * (aCollisionNorm * aObjToCheck.mVelocity);
		aObjToCheck.mVelocity -= reflect * 0.75f; // Hardcoded in 0.75f, though a variable for bounciness on each object would be better I think
	}
}

void CollisionManager::SAT(Geo::GeoObjPhysics& a, Geo::GeoObjPhysics& b, float& aOutPenetration, glm::vec3& aOutNormal)
{
	/* Find which side was hit */ 
	//(Separating Axis Theorem) Thanks to for getting me started: http://www.randygaul.net/category/collision/
	glm::vec3 t = a.GetPosition() - b.GetPosition();

	float axExtent = (a.GetCollider().GetBoundsMax().x - a.GetCollider().GetBoundsMin().x) / 2;
	float bxExtent = (b.GetCollider().GetBoundsMax().x - b.GetCollider().GetBoundsMin().x) / 2;
	float xOverlap = axExtent + bxExtent - abs(t.x);

	float ayExtent = (a.GetCollider().GetBoundsMax().y - a.GetCollider().GetBoundsMin().y) / 2;
	float byExtent = (b.GetCollider().GetBoundsMax().y - b.GetCollider().GetBoundsMin().y) / 2;
	float yOverlap = ayExtent + byExtent - abs(t.y);

	float azExtent = (a.GetCollider().GetBoundsMax().z - a.GetCollider().GetBoundsMin().z) / 2;
	float bzExtent = (b.GetCollider().GetBoundsMax().z - b.GetCollider().GetBoundsMin().z) / 2;
	float zOverlap = azExtent + bzExtent - abs(t.z);

	/* Find the axis with the least amount of overlap */
	if (xOverlap < yOverlap && xOverlap < zOverlap) // Hit on left or right side
	{
		aOutPenetration = xOverlap;
		aOutNormal = glm::vec3(1.0f, 0.0f, 0.0f);
		if (t.x > 0)
			aOutNormal = glm::vec3(-1.0f, 0.0f, 0.0f);
	}
	if (yOverlap < xOverlap && yOverlap < zOverlap) // Hit on top or bottom side
	{
		aOutPenetration = yOverlap;
		aOutNormal = glm::vec3(0.0f, 1.0f, 0.0f);
		if (t.y > 0)
			aOutNormal = glm::vec3(0.0f, -1.0f, 0.0f);
	}
	if (zOverlap < xOverlap && zOverlap < yOverlap) // Hit on front or back side
	{
		aOutPenetration = zOverlap;
		aOutNormal = glm::vec3(0.0f, 0.0f, 1.0f);
		if (t.z > 0)
			aOutNormal = glm::vec3(0.0f, 0.0f, -1.0f);
	}

	//OUTPUT TEST
	/*
	if (normal.x > 0)
		cout << "Hit right" << endl;
	else if (normal.x < 0)
		cout << "Hit left" << endl;
	else if (normal.y > 0)
		cout << "Hit top" << endl;
	else if (normal.y < 0)
		cout << "Hit bottom" << endl;
	else if (normal.z > 0)
		cout << "Hit front" << endl;
	else if (normal.z < 0)
		cout << "Hit back" << endl;
	else
		cout << "Hit nothing!?!" << endl;
	*/
}

void CollisionManager::ApplyGravityAll()
{
	for (std::list<Geo::GeoObjPhysics*>::iterator i = mPhysicsObjects.begin(); i != mPhysicsObjects.end(); ++i)
		if ((**i).IsUseGravity())
			ApplyGravity((**i));
}

void CollisionManager::ApplyGravity(Geo::GeoObjPhysics& aObj)
{
	float dt = GLUtils::Time::GetDeltaTime();
	aObj.mVelocity += glm::vec3(GlobalProperties::GRAVITY[0] * dt, GlobalProperties::GRAVITY[1] * dt, GlobalProperties::GRAVITY[2] * dt);
	aObj.mVelocity *= 1 - GlobalProperties::DRAG;
}

void CollisionManager::AddPhysicsObject(Geo::GeoObjPhysics& aObj)
{
	mPhysicsObjects.push_back(&aObj);

	#ifdef USE_OCTREE
	mOctree.Add(&aObj.mCollider);
	#endif
}

void CollisionManager::RewindEnd()
{
	for (std::list<Geo::GeoObjPhysics*>::iterator i = mPhysicsObjects.begin(); i != mPhysicsObjects.end(); ++i)
	{
		(**i).mVelocity = glm::vec3(0); // In reality, should be saving off the velocities and re applying them instead of this...
		if ((**i).mMass != 0.0f && !(**i).mIsKinematic) (**i).WakeUp();
	}
}