#include "Octree.h"

Octree::Octree(glm::vec3 aMin, glm::vec3 aMax, int aDepth) : mMin(aMin), mMax(aMax), mDepth(aDepth), mColliderCount(0), mHasChildren(false)
{
	mCenter = (mMin + mMax) / 2.0f;
	mWireframeBox.Init(mMin, mMax);
}

Octree::~Octree()
{
	if (mHasChildren)
		DestroyChildren();
}

void Octree::File(Collider* aCol, glm::vec3 aPos, bool aAdd)
{
	if (!aCol)
		return;

	for (int x = 0; x < 2; ++x)
	{
		if (x == 0)
		{
			if (aPos.x - aCol->GetBoundsMax().x > mCenter.x)
				continue;
		}
		else if (aPos.x + aCol->GetBoundsMax().x < mCenter.x)
			continue;


		for (int y = 0; y < 2; ++y)
		{
			if (y == 0)
			{
				if (aPos.y - aCol->GetBoundsMax().y > mCenter.y)
					continue;
			}
			else if (aPos.y + aCol->GetBoundsMax().y < mCenter.y)
				continue;


			for (int z = 0; z < 2; ++z)
			{
				if (z == 0)
				{
					if (aPos.z - aCol->GetBoundsMax().z > mCenter.z)
						continue;
				}
				else if (aPos.z + aCol->GetBoundsMax().z < mCenter.z)
					continue;

				if (aAdd)
					mChildren[x][y][z]->Add(aCol);
				else
					mChildren[x][y][z]->Remove(aCol, aPos);

			}
		}
	}
}

void Octree::CreateChildren()
{
	for (int x = 0; x < 2; ++x)
	{
		float minX;
		float maxX;
		if (x == 0)
		{
			minX = mMin.x;
			maxX = mCenter.x;
		}
		else
		{
			minX = mCenter.x;
			maxX = mMax.x;
		}

		for (int y = 0; y < 2; ++y)
		{
			float minY;
			float maxY;
			if (y == 0)
			{
				minY = mMin.y;
				maxY = mCenter.y;
			}
			else
			{
				minY = mCenter.y;
				maxY = mMax.y;
			}


			for (int z = 0; z < 2; ++z)
			{
				float minZ;
				float maxZ;
				if (z == 0)
				{
					minZ = mMin.z;
					maxZ = mCenter.z;
				}
				else
				{
					minZ = mCenter.z;
					maxZ = mMax.z;
				}

				mChildren[x][y][z] = new Octree(glm::vec3(minX, minY, minZ), glm::vec3(maxX, maxY, maxZ), mDepth+1);

			}
		}
	}

	//Put all mColliders into new children
	for (std::set<Collider*>::iterator i = mColliders.begin(); i != mColliders.end(); ++i)
		File((*i), (*i)->GetCenter(), true);

	mColliders.clear();
	mHasChildren = true;
}

void Octree::Collect(std::set<Collider*>& aSet)
{
	if (mHasChildren) // If you have children, get all of their colliders
	{
		for (int x = 0; x < 2; ++x)
			for (int y = 0; y < 2; ++y)
				for (int z = 0; z < 2; ++z)
					mChildren[x][y][z]->Collect(aSet);
	}
	else // Otherwise, just take ours
	{
		for (std::set<Collider*>::iterator i = mColliders.begin(); i != mColliders.end(); ++i)
			aSet.insert((*i));
	}
}

void Octree::DestroyChildren()
{
	// Take all childrens colliders, delete them, and now no longer have children
	Collect(mColliders);

	for (int x = 0; x < 2; ++x)
		for (int y = 0; y < 2; ++y)
			for (int z = 0; z < 2; ++z)
				delete mChildren[x][y][z];

	mHasChildren = false;
}

void Octree::Remove(Collider* aCol, glm::vec3 aPos)
{
	if (!aCol)
		return;

	--mColliderCount;

	if (mHasChildren)
	{
		if (mColliderCount < MIN_COLLIDERS_PER_OCTREE)
			DestroyChildren();
		else
			File(aCol, aPos, false);
	}
	else
		mColliders.erase(aCol);
}

void Octree::Remove(Collider* aCol)
{
	if (!aCol)
		return;

	Remove(aCol, aCol->GetCenter());
}

void Octree::Add(Collider* aCol)
{
	if (!aCol)
		return;

	++mColliderCount;
	if (!mHasChildren && mDepth < MAX_OCTREE_DEPTH && mColliderCount > MAX_COLLIDERS_PER_OCTREE)
		CreateChildren();

	if (mHasChildren)
		File(aCol, aCol->GetCenter(), true);
	else
		mColliders.insert(aCol);
}

void Octree::MoveCollider(Collider* aCol, glm::vec3 aOldPos)
{
	Remove(aCol, aOldPos);
	Add(aCol);
}

void Octree::PotentialCollisions(std::vector<CollliderPair>& aVec)
{
	if (mHasChildren) // If you have children, get all of their colliders
	{
		for (int x = 0; x < 2; ++x)
			for (int y = 0; y < 2; ++y)
				for (int z = 0; z < 2; ++z)
					mChildren[x][y][z]->PotentialCollisions(aVec);
	}
	else // Otherwise, just take ours
	{
		for (std::set<Collider*>::iterator i = mColliders.begin(); i != mColliders.end(); ++i)
			for (std::set<Collider*>::iterator j = mColliders.begin(); j != mColliders.end(); ++j)
				if ((*i) < (*j)) // TODO, use collision managers CheckForCollisions looping method so this isn't neccesary!
				{
					CollliderPair cp;
					cp.a = (*i);
					cp.b = (*j);
					aVec.push_back(cp);
				}
	}
}

void Octree::Draw()
{
	if (mHasChildren)
	{
		for (int x = 0; x < 2; ++x)
			for (int y = 0; y < 2; ++y)
				for (int z = 0; z < 2; ++z)
					mChildren[x][y][z]->Draw();
	}
	else
		mWireframeBox.Draw();
}