#include "pch.h"
#include "QuadTree.h"

//Static vector holding all nodes in the current tree for easy access by the worker threads
Concurrency::concurrent_vector anx::QuadTree::pNodes{};

anx::QuadTree::QuadTree()
	:m_Level(-1), m_pNodes{nullptr, nullptr, nullptr, nullptr}, m_pDynamicObjects(Concurrency::concurrent_vector{}),
	m_pStaticObjects(Concurrency::concurrent_vector{}), m_MaxLevel(5)
{
}

anx::QuadTree::~QuadTree()
{
}

//Function that builds the entire tree, give the level bounds and the number of levels in the tree
anx::QuadTree * anx::QuadTree::BuildTree(const Rect & bounds, int levels)
{
	pNodes.reserve(500);

	QuadTree* pRootNode = ObjectPool::GetInstance().RequestObject();
	pRootNode->Initialize(0, bounds);
	pRootNode->m_MaxLevel = levels;
	std::queue pQueue;
	pQueue.push(pRootNode);

	while(!pQueue.empty())
	{
		pQueue.front()->Split(levels, pQueue);
		pQueue.pop();
	}

	return pRootNode;
}

//Function makes sure all our children are set to nullptr and the current node knows the number of total levels in the tree
void anx::QuadTree::Initialize(int level, const Rect & bounds)
{
	for (int i = 0; i < 4; ++i)
	{
		m_pNodes[i] = nullptr;
	}

	m_Level = level;
	m_Bounds = bounds;
}

void anx::QuadTree::Clear()
{
	//Clear all nodes from the node list
	m_pStaticObjects.clear();
	m_pDynamicObjects.clear();
	for (int i = 0; i < 4; ++i)
	{
		if(m_pNodes[i] != nullptr)
		{
			m_pNodes[i]->Clear();
		}
	}
	pNodes.clear();
	m_Size = 0;
}

//Splits up a node into four sub nodes with each taking care of a quarter of the current node's bounds
void anx::QuadTree::Split()
{
	if (m_pNodes[0]) return;

	float subWidth = m_Bounds.width * 0.5f;
	float subHeight = m_Bounds.height * 0.5f;
	const Vector2& topLeft = m_Bounds.topLeft;

	m_pNodes[0] = ObjectPool::GetInstance().RequestObject();
	m_pNodes[0]->Initialize(m_Level + 1, Rect(topLeft, subWidth, subHeight));

	m_pNodes[1] = ObjectPool::GetInstance().RequestObject();
	m_pNodes[1]->Initialize(m_Level + 1, Rect({ topLeft.x + subWidth, topLeft.y }, subWidth, subHeight));

	m_pNodes[2] = ObjectPool::GetInstance().RequestObject();
	m_pNodes[2]->Initialize(m_Level + 1, Rect({ topLeft.x, topLeft.y + subHeight }, subWidth, subHeight));

	m_pNodes[3] = ObjectPool::GetInstance().RequestObject();
	m_pNodes[3]->Initialize(m_Level + 1, Rect({ topLeft.x + subWidth, topLeft.y + subHeight }, subWidth, subHeight));
}

//Base function called when the tree is being built
void anx::QuadTree::Split(int maxLevel, std::queue& pQueue)
{
	if (m_Level < maxLevel)
	{
		Split();
		for (int i = 0; i < 4; ++i)
		{
			pQueue.push(m_pNodes[i]);
		}
	}
}

//Returns the index of the child node the rectangle completely fits into
//Or -1 is the rectangle overlaps more than one sub node
int anx::QuadTree::GetIndex(const Rect& collider)
{
	for (int i = 0; i < 4; ++i)
	{
		const Rect& bounds = m_pNodes[i]->GetBounds();
		if (collider.IsInside(bounds))
			return i;
	}
	return -1;
}

//Finds the correct node to add the collider to, if the collider overlaps multiple nodes, it gets added in all of the overlapping nodes
void anx::QuadTree::Insert(const Rect& colliderBounds, ColliderComponent * pCollider)
{

	if(m_pNodes[0] != nullptr)
	{
		for (int i = 0; i < 4; ++i)
		{
			Rect bounds = m_pNodes[i]->GetBounds();
			if (colliderBounds.IsOverlapping(bounds))
				m_pNodes[i]->Insert(colliderBounds, pCollider);
		}
	}
	else
	{
		//Add the colliders in the correct queue, the tree supports both static and dynamic colliders
		if(pCollider->GetIsStatic())
			m_pStaticObjects.push_back(pCollider);
		else
			m_pDynamicObjects.push_back(pCollider);

		if (++m_Size == 2)
			pNodes.push_back(this);
	}

}

const anx::Rect & anx::QuadTree::GetBounds()
{
	return m_Bounds;
}

const Concurrency::concurrent_vector& anx::QuadTree::GetDynamicObjects() const
{
	return m_pDynamicObjects;
}

const Concurrency::concurrent_vector& anx::QuadTree::GetStaticObjects() const
{
	return m_pStaticObjects;
}

Concurrency::concurrent_vector& anx::QuadTree::GetNotEmptyNodes()
{
	return pNodes;
}

int anx::QuadTree::GetNrTotalNodes()const
{
	return pNodes.size();
}

int anx::QuadTree::GetNrNodes()
{
	return int(std::pow(4,m_MaxLevel));
}