#pragma once
#include <deque>
#include <vector>
#include <string>
#include <unordered_map>
#include <fstream>
#include <algorithm>
#include <unordered_set>
#include <numeric>
#include <functional>
#include "BaseTree.h"
#include "../../inc/tbb/parallel_sort.h"
#include "../../inc/tbb/parallel_for_each.h"
#include "../../inc/tbb/concurrent_queue.h"
#include "../../core/Defines.h"
#include "../../core/CollectionHelper.h"
#include "../../core/Serializer.h"
#include "../../core/Util/BoolArray.h"
#include "../../core/Util/ObjectPool.h"

template<typename NodeType = Node>
class Tree : public BaseTree {
public:
	Tree(unsigned8 maxLevel) :
		BaseTree(),
		mLeafNode(0),
		mLeafsAreEqual(true), // Default tree nodes are equal. Override the root for trees that don't have this property
		mMaxLevel(maxLevel),
		mNodePool(ObjectPool<NodeType>(/*1000000*/)),
		mSortedOnLevel(true)
	{
		// Create the root
		Create(0);
	}
	
	~Tree() override
	{
		Clear();
	}

	const Node*					GetNode(const unsigned32 index) const override	{ return mNodePool[index]; }
	Node*						GetNode(const unsigned32 index)	override		{ return mNodePool[index]; }
	inline const NodeType*		GetTypedNode(const unsigned32 index) const		{ return mNodePool[index]; }
	inline NodeType*			GetTypedNode(const unsigned32 index)			{ return mNodePool[index]; }
	inline const NodeType*		GetRoot() const									{ return mNodePool[0]; /*The first node in the nodepool is ALWAYS the root.*/ }
	inline NodeType*			GetRoot()										{ return mNodePool[0]; }
	
	inline unsigned8			GetMaxLevel() const override { return mMaxLevel; }
	unsigned32					GetNodeCount() const override { return (unsigned32)mNodePool.Size(); }
	inline bool					IsEmpty() const override { return !GetRoot()->HasChildren(); }

	bool						HasLeaf(const glm::uvec3& coord) const { return HasNode(coord, GetMaxLevel()); }
	NodeType*					AddLeafNode(const glm::uvec3& coord) { return AddNode(coord, GetMaxLevel()); }
	bool						HasNode(const glm::uvec3& coord, const unsigned8 level) const { return GetRoot()->HasNode(coord, level); }
	NodeType*					AddNode(const glm::uvec3& coord, const unsigned8 level) { 
		NodeType* root = GetRoot();
		return (NodeType*)(root->AddNode(coord, level));
	}

	void Traverse(const std::function<void(const Node*)>& f) const { GetRoot()->Traverse(f); }

	std::vector<size_t> GetNodesPerLevel() const override
	{
		std::vector<size_t> nodesPerLevel;
		nodesPerLevel.resize(GetMaxLevel() + 1, 0);
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
			nodesPerLevel[GetTypedNode(i)->GetLevel()]++;
		return nodesPerLevel;
	}
	size_t GetNodesInLevel(const unsigned8 level) const
	{
		size_t res = 0;
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
			if (GetTypedNode(i)->GetLevel() == level) res++;
		return res;
	}
	virtual std::vector<size_t> GetOctreeNodesPerLevel() const override
	{
		std::vector<size_t> octreeNodesPerLevel(GetMaxLevel() + 1);
		std::function<void(const Node*)> nodeCounter = [&octreeNodesPerLevel](const Node* node) -> void
		{
			octreeNodesPerLevel[node->GetLevel()]++;
		};
		this->Traverse(nodeCounter);
		return octreeNodesPerLevel;
	}

	virtual unsigned64 GetPointerCount() const override
	{
		unsigned64 res = 0;
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
			res += GetTypedNode(i)->GetChildCount();
		return res;
	}

	// Returns a map with for each node who it's parents are (O(N)). To find the parents of a node, use it's index (node->GetIndex())
	std::vector<std::vector<std::pair<unsigned, ChildIndex>>> GetParentMap() const
	{
		std::vector<std::vector<std::pair<unsigned, ChildIndex>>> parentMap(GetNodeCount(), std::vector<std::pair<unsigned, ChildIndex>>());
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
		{
			NodeType* node = GetTypedNode(i);
			for (ChildIndex c = 0; c < 8; c++)
				if (node->HasChild(c))
					parentMap[node->GetChildIndex(c)].push_back(std::make_pair(i, c));
		}
		return parentMap;
	}

	// Counts the number of parents each node has (e.g. how many pointers there are to each node). For a regular octree, this will be 1, but for a DAG it isn't necessarily.
	std::vector<size_t> GetParentCounts() const override
	{
		std::vector<size_t> parentCounts(GetNodeCount(), 0);
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
		{
			const NodeType* node = GetTypedNode(i);
			unsigned32* children = node->GetChildren();
			for (ChildIndex c = 0; c < node->GetChildCount(); c++)
				parentCounts[children[c]]++;
		}
		return parentCounts;
	}

	unsigned64 GetLeafVoxelCount() const
	{
		std::vector<unsigned64> leafCountTable(GetNodeCount(), 1);
		for (unsigned8 level = GetMaxLevel(); level-- > 0;)
		{
			for (unsigned32 i = 0; i < GetNodeCount(); i++)
			{
				const NodeType* node = GetTypedNode(i);
				if (node->GetLevel() == level)
				{
					unsigned64 sum = 0;
					unsigned32* children = node->GetChildren();
					for (ChildIndex c = 0; c < node->GetChildCount(); c++)
						sum += leafCountTable[children[c]];
					leafCountTable[i] = sum;
				}
			}
		}
		return leafCountTable[0];
	}
	
	bool ReadTree(const char* fileName) override
	{
		std::string binFileName = GetOctreeFileName(fileName);
		std::ifstream treeInFile(binFileName, std::ios::binary);

		if (treeInFile.good()) {
			bool succes = Deserialize(treeInFile);

			// Finished reading
			treeInFile.close();
			return succes;
		}

		return false;
	}
	bool WriteTree(const char* fileName) override
	{
		std::string binFileName = GetOctreeFileName(fileName);
		std::ofstream treeOutFile(binFileName, std::ios::binary);

		bool succes = Serialize(treeOutFile);

		treeOutFile.close();
		return succes;
	}

	bool VerifyTree(const char* fileName) override
	{
		std::string binFileName = GetOctreeFileName(fileName);
		std::ifstream treeInFile(binFileName, std::ios::binary);

		if (treeInFile.good()) {
			// Read the maximum level in the tree
			unsigned8 maxLevel;
			Serializer<unsigned8>::Deserialize(maxLevel, treeInFile);
			this->mMaxLevel = maxLevel;

			// Read the nodes per level
			std::vector<unsigned> nodesPerLevel(maxLevel + 1);
			Serializer<unsigned*>::Deserialize(&nodesPerLevel[0], mMaxLevel + 1, treeInFile);

			// Sum the nodes per level to get the total number of nodes
			size_t nodeCount = std::accumulate(nodesPerLevel.begin(), nodesPerLevel.end(), 0);

			printf(".");

			// Read the childmasks for all nodes (needed to know which pointers exist)
			std::vector<ChildMask> childmasks(nodeCount);
			Serializer<ChildMask*>::Deserialize(&childmasks[0], nodeCount, treeInFile);
			printf(".");

			// Read all pointers
			unsigned* newChildren = new unsigned[8];
			for (ChildMask mask : childmasks)
			{
				if (treeInFile.eof()) return false;
				ChildIndex childCount = mask.GetSet();
				Serializer<unsigned*>::Deserialize(newChildren, childCount, treeInFile);

				for (ChildIndex i = 0; i < childCount; i++)
				{
					if (newChildren[i] >= nodeCount)
					{
						printf("Node index out of bounds: %u", newChildren[i]);
						delete[] newChildren;
						return false;
					}
				}
			}
			delete[] newChildren;
			printf("..");

			// Finished reading
			treeInFile.close();

			return true;
		}
		return false;
	}

	bool Serialize(std::ostream& treeOutFile)
	{
		auto levelIndices = SortOnLevel();

		// Write the total number of levels in the tree
		unsigned maxLevel = GetMaxLevel();
		Serializer<unsigned8>::Serialize(maxLevel, treeOutFile);

		// Write the nodes per level
		unsigned nodesThisLevel;
		for (unsigned8 level = 0; level < maxLevel + 1; level++)
		{
			nodesThisLevel = (unsigned)(levelIndices[level + 1] - levelIndices[level]);
			Serializer<unsigned32>::Serialize(nodesThisLevel, treeOutFile);
		}

		// Gather the child masks for all nodes:
		std::vector<ChildMask> childMasks(GetNodeCount());
		tbb::parallel_for((unsigned32)0, GetNodeCount(), [&](unsigned32 i)
		{
			childMasks[i] = GetTypedNode(i)->GetChildmask();
		});

		Serializer<ChildMask*>::Serialize(&childMasks[0], childMasks.size(), treeOutFile);

		// Write child pointers of all nodes
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
		{
			NodeType* node = GetTypedNode(i);
			ChildMask mask = node->GetChildmask();
			unsigned32* children = node->GetChildren();
			Serializer<unsigned32*>::Serialize(children, mask.GetSet(), treeOutFile);
		}

		// Hack: first write the root properties, then the tree properties, then the rest of the nodes
		// This is done to be compliant with old files of previous versions

		GetRoot()->WriteProperties(treeOutFile);
		WriteProperties(treeOutFile);
		// Write extra properties per node
		for (unsigned32 i = 1; i < GetNodeCount(); i++)
			GetTypedNode(i)->WriteProperties(treeOutFile);

		return true;
	}

	bool Deserialize(std::istream& treeInFile)
	{
		this->Clear();
		this->InitializeReadTree();

		// Read the maximum level in the tree
		Serializer<unsigned8>::Deserialize(mMaxLevel, treeInFile);

		// Read the nodes per level
		std::vector<unsigned> nodesPerLevel(mMaxLevel + 1);
		Serializer<unsigned*>::Deserialize(&nodesPerLevel[0], mMaxLevel + 1, treeInFile);

		// Sum the nodes per level to get the total number of nodes
		unsigned32 nodeCount = std::accumulate(nodesPerLevel.begin(), nodesPerLevel.end(), 0);

		printf(".");

		// Read the childmasks for all nodes (needed to know which pointers exist)
		std::vector<ChildMask> childmasks(nodeCount);
		Serializer<ChildMask*>::Deserialize(&childmasks[0], nodeCount, treeInFile);

		for (unsigned8 level = 0; level <= mMaxLevel; level++)
			for (unsigned i = 0; i < nodesPerLevel[level]; i++)
				// Create the node. Creating them in order also adds them to the node pool in order.
				Create(level);
		printf(".");

		// Read all pointers
		unsigned* newChildren = new unsigned[8];
		for (unsigned32 i = 0; i < nodeCount; i++)
		{
			ChildMask mask = childmasks[i];
			Serializer<unsigned*>::Deserialize(newChildren, mask.GetSet(), treeInFile);
			GetTypedNode(i)->SetChildren(mask, newChildren);
		}
		delete[] newChildren;
		printf(".");

		GetRoot()->ReadProperties(treeInFile);
		ReadProperties(treeInFile);
		// Read extra properties per node
		for (unsigned32 i = 1; i < GetNodeCount(); i++)
		{
			if (treeInFile.eof())
			{
				printf("Something is wrong...");
				return false;
			}
			GetTypedNode(i)->ReadProperties(treeInFile);
		}
		printf(".");

		this->FinalizeReadTree();
		return true;
	}

	static std::string			GetOctreeFileName(const char* fileName) { return std::string(fileName) + ".oct"; }
	static std::string			GetAdditionalPoolFileName(const char* fileName) { return std::string(fileName) + ".additional.pool"; }

	virtual void ReadProperties(std::istream& file) {}
	virtual void WriteProperties(std::ostream& file) {}

	unsigned8				GetAdditionalTreeInfoSize() const override { return 0; }
	std::vector<unsigned8>	GetAdditionalTreeInfo(const std::vector<size_t>& nodePointers) const override { return std::vector<unsigned8>(); }
	unsigned8				GetAdditionalBytesPerNode(unsigned8 level) const override { return 0; }
	std::vector<unsigned8>	GetAdditionalNodeBytes(const Node* node) const override { return std::vector<unsigned8>(); }
	bool					LastChildHasAdditionalBytes() const override { return true; }
	unsigned8				GetAdditionalBytesPerPointer(unsigned8 level) const override { return 0; }
	std::vector<unsigned8>	GetAdditionalPointerBytes(const Node* node, ChildIndex child) const override { return std::vector<unsigned8>(); }

	// Reads the node pool and stores it in this tree
	bool ReadAdditionalPool(const char* fileName) override
	{
		if (!HasAdditionalPool()) return true;
		std::string binFileName = GetAdditionalPoolFileName(fileName);
		std::ifstream additionalPoolInFile(binFileName, std::ios::binary);

		if (additionalPoolInFile.good()) {
			// Destroy whole current node pool
			ReadAdditionalPoolProperties(additionalPoolInFile);
			additionalPoolInFile.close();
			return true;
		}
		return false;
	}
	// Writes the node pool
	bool WriteAdditionalPool(const char* fileName) override
	{
		if (!HasAdditionalPool()) return true;
		std::string binFileName = GetAdditionalPoolFileName(fileName);
		std::ofstream additionalPoolOutFile(binFileName, std::ios::binary);
		WriteAdditionalPoolProperties(additionalPoolOutFile);
		additionalPoolOutFile.close();
		return true;
	}
	virtual bool HasAdditionalPool() const { return false; }

	// Clears the whole tree, including the root!
	virtual void Clear() override
	{
		mNodePool.Clear();
	}

	// Removed all nodes marked for deletion and updates the indices.
	void Clean()
	{
		unsigned32 oldNodeCount = GetNodeCount();
		mNodePool.Clean();
		UpdateNodeIndices(oldNodeCount);
	}

	NodeType* Create(unsigned8 level) override
	{
		// Make sure only one leaf node is created (memory, performance)
		if (mLeafsAreEqual && level == GetMaxLevel() && mLeafNode != 0)
			return GetTypedNode(mLeafNode);
		// Otherwise, create a new node
		NodeType* res = mNodePool.Create(this, level);
		mSortedOnLevel = false;

		res->SetIndex(GetNodeCount() - 1);
		if (mLeafsAreEqual && level == GetMaxLevel() && mLeafNode == 0) mLeafNode = res->GetIndex();
		return res;
	}

	void Append(glm::uvec3 coordinates, unsigned8 level, Tree* tree)
	{
		// Let some possible inhereting class do preprocessing on the current tree before append
		AppendPreProcess(coordinates, level, tree);

		// First create/get the node that acts as the root of the tree to append
		std::vector<unsigned32> equivalents(tree->GetNodeCount());
		NodeType* treeRoot = AddNode(coordinates, level);
		equivalents[0] = treeRoot->GetIndex();

		// Make copies of all nodes in tree, and add them to our own nodepool (with the correct level and root)
		for (unsigned32 i = 1; i < tree->GetNodeCount(); i++)
		{
			NodeType* node = tree->GetTypedNode(i);
			NodeType* copy = this->Create(node->GetLevel() + level);
			equivalents[i] = copy->GetIndex();
		}

		unsigned32* newChildren = new unsigned32[8];
		// Restore all child pointers
		for (unsigned32 i = 0; i < tree->GetNodeCount(); i++)
		{
			NodeType* node = tree->GetTypedNode(i);
			unsigned32* children = node->GetChildren();
			unsigned8 childCount = node->GetChildCount();
			for (ChildIndex c = 0; c < childCount; c++) newChildren[c] = equivalents[children[c]];
			NodeType* copy = this->GetTypedNode(equivalents[i]);
			copy->SetChildren(node->GetChildmask(), newChildren);
		}
		delete[] newChildren;

		// Copy properties
		tbb::parallel_for((unsigned32)0, tree->GetNodeCount(), [&](unsigned i)
		{
			NodeType* source = tree->GetTypedNode(i);
			GetTypedNode(equivalents[i])->CopyProperties(source);
		});

		// Let some possible inhereting class do postprocessing on the added nodes
		AppendPostProcess(coordinates, level, tree);
	}

	// Appends the given tree to this tree. The nodes in the original tree will be moved to this tree.
	// During the appending, nodes are automatically merged when possible to keep memory usage low.
	// Note that this DOES NOT call AppendPostProcess, so trees that depend on this cannot be build using this method
	void AppendAndMerge(glm::uvec3 coordinates, unsigned8 appendLevel, Tree* tree)
	{
		// Let some possible inhereting class do preprocessing on the current tree before append
		AppendPreProcess(coordinates, appendLevel, tree);

		this->SortNodes();
		std::vector<unsigned32> levelIndices = this->SortOnLevel();
		std::vector<unsigned32> appendedTreeLevelIndices = tree->SortOnLevel();
		
		// First create/get the node that acts as the root of the tree to append
		NodeType* treeRoot = this->AddNode(coordinates, appendLevel);

		// Contains at the index of the node in the tree that is to be appended, the index of the replacement node in the pool
		std::vector<unsigned32> replacementMap(tree->GetNodeCount(), 0);
		replacementMap[0] = treeRoot->GetIndex();
		for (unsigned8 level = GetMaxLevel(); level > appendLevel; level--)
		{
			// Sort all the nodes in the current level of the other tree
			unsigned32 levelStart = levelIndices[level];
			unsigned32 levelEnd = levelIndices[level + 1];
			unsigned32 appendedLevelStart = appendedTreeLevelIndices[level - appendLevel];
			unsigned32 appendedLevelEnd = appendedTreeLevelIndices[level - appendLevel + 1];

			//std::vector<NodeType&> existingNodes(levelEnd - levelStart);
			//for (unsigned32 i = levelStart; i < levelEnd; i++) existingNodes[i - levelStart] = GetTypedNode(i);

			//tbb::parallel_sort(existingNodes.begin(), existingNodes.end(), NodeComparer());

			// Vector of all nodes that need to be appended
			std::vector<unsigned32> toAppend(appendedLevelEnd - appendedLevelStart);
			for (unsigned32 i = appendedLevelStart; i < appendedLevelEnd; i++) toAppend[i - appendedLevelStart] = i;

			tbb::parallel_for_each(toAppend.begin(), toAppend.end(), [&](const unsigned32 i)
			{
				NodeType* node = tree->GetTypedNode(i);
				unsigned32* children = node->GetChildren();
				ChildIndex childCount = node->GetChildCount();
				// Make sure the node looks exactly like how it would look if it was in the current tree:
				for (ChildIndex c = 0; c < childCount; c++)
					children[c] = replacementMap[children[c]];
				node->SetLevel(level);
				node->MoveToTree(this);
			});

			// Sort the nodes in the same way the existing nodes are sorted
			//NodeComparer comparer();
			tbb::parallel_sort(toAppend.begin(), toAppend.end(), [&](const unsigned32 i1, const unsigned32 i2)
			{
				NodeType* node1 = tree->GetTypedNode(i1);
				NodeType* node2 = tree->GetTypedNode(i2);
				return node1->Compare(*node2);
			});

			// Go through the nodes in this order.
			unsigned32 existingIndex = 0;
			for (const unsigned32 i : toAppend)
			{
				NodeType* node = tree->GetTypedNode(i);
				// Scan the existing nodes until they are no longer smaller than the current node
				while (existingIndex < (levelEnd - levelStart) && GetTypedNode(levelStart + existingIndex)->Compare(*node))
					existingIndex++;
				// If the node at that position is equal to the current node, then delete the current node
				if (existingIndex < (levelEnd - levelStart) && GetTypedNode(levelStart + existingIndex)->Equals(*node))
				{
					replacementMap[node->GetIndex()] = levelStart + existingIndex;
					tree->Destroy(node);
				}
				else // Otherwise, add this node to the pool
				{
					// Create a copy of the node
					node = mNodePool.Add(node);
					tree->Destroy(node->GetIndex());
					unsigned32 newNodeIndex = GetNodeCount() - 1;
					replacementMap[node->GetIndex()] = newNodeIndex;
					node->SetIndex(newNodeIndex);
					mSortedOnLevel = false;
				}
			}
		}

		// Reconnect the children of the replacement root
		unsigned8 childCount = tree->GetRoot()->GetChildCount();
		unsigned32* children = tree->GetRoot()->GetChildren();
		for (ChildIndex c = 0; c < childCount; c++)
			children[c] = replacementMap[children[c]];
		treeRoot->SetChildren(tree->GetRoot()->GetChildmask(), children);

		tree->Clear();
	}

	// Moves the given tree to the this tree, replacing the contents of the this try by that of the given tree.
	// Note that all nodes are recreated, because a tree with a different subclass of node may be moved to this one.
	// This means that all properties of the original tree will be lost!
	void MoveShallow(BaseTree* tree)
	{
		// Update the current tree to be similar to the source tree
		Clear();
		this->mMaxLevel = tree->GetMaxLevel();

		// Create copies of the old tree and put them in the new tree, with correct childpointers
		for (unsigned32 i = 0; i < tree->GetNodeCount(); i++)
		{
			Node* original = tree->GetNode(i);
			NodeType* replacer = mNodePool.Create(this, original->GetLevel());
			replacer->SetChildren(original->GetChildmask(), original->GetChildren());
			//tree->Destroy(i);
		}

		// Clear the original tree
		tree->Clear();
	}

	// Sorts the nodes on their level, and returns a list of start indices per level
	std::vector<unsigned32>	SortOnLevel() override
	{
		// Sort nodes on level (ignore the root = first node in pool)
		if (!mSortedOnLevel)
		{
			//mNodePool.Sort(NodeComparer());
			mNodePool.Sort(1, GetNodeCount(), [](const NodeType& a, const NodeType& b) { return a.GetLevel() < b.GetLevel(); });
			//tbb::parallel_sort(mNodePool.begin() + 1, mNodePool.end(), [](NodeType* a, NodeType* b) { return a->GetLevel() < b->GetLevel(); });

			UpdateNodeIndices(GetNodeCount());

			mSortedOnLevel = true;
		}

		// Find the start indices of all levels in the node pool
		std::vector<unsigned32> levelIndices(GetMaxLevel() + 1);
		unsigned8 curLevel = 255;
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
		{
			NodeType* node = GetTypedNode(i);
			if (node->GetLevel() != curLevel) {
				curLevel = node->GetLevel();
				levelIndices[curLevel] = node->GetIndex();
			}
		}
		// If for some reason, there are no nodes on every level, fill the rest of the level offsets as if there were these nodes...
		for (unsigned8 level = curLevel + 1; level <= GetMaxLevel(); level++) levelIndices[level] = GetNodeCount();

		// Add a dummy value at the end
		levelIndices.push_back(GetNodeCount());

		return levelIndices;
	}

	void SortNodes()
	{
		std::vector<unsigned32> levelIndices = SortOnLevel();
		for (unsigned8 level = 1; level <= GetMaxLevel(); level++) this->SortBetween(levelIndices[level], levelIndices[level + 1], NodeComparer());
		UpdateNodeIndices(GetNodeCount());
	}

	void ToDAG(unsigned8 fromLevel = 0, bool verbose = true)
	{
		if (this->GetNodeCount() == 1)
			return; // Empty tree = DAG

		// Sort the nodes on level (for quick access of nodes within a level).
		// Note that we can't sort on the node comparer just yet, as the nodes will change during the ToDAG process.
		auto levelIndices = SortOnLevel();

		mMaxLevel = (unsigned8)(levelIndices.size() - 2);
		if (fromLevel > GetMaxLevel()) return;

		unsigned32 maxOldIndex = GetNodeCount() - 1;
		
		// Run through the layers in reverse order and compress them bottom up
		for (unsigned8 level = GetMaxLevel(); level > fromLevel; --level)
		{
			if (verbose) printf(".");
			// Initialize some variables needed
			unsigned32 levelStartIndex = levelIndices[level];
			unsigned32 levelEndIndex = levelIndices[level + 1];
			unsigned32 parentLevelStartIndex = levelIndices[level - 1];
			unsigned32 levelNodeCount = levelEndIndex - levelStartIndex;
			unsigned32 deletedCount = 0;

			bool fullRead = !mLeafsAreEqual || level != GetMaxLevel();

			if (fullRead)
			{
				// Sort nodes using the node comparer
				mNodePool.Sort(levelStartIndex, levelEndIndex, NodeComparer());
				if (verbose) printf(".");

				// Find unique nodes and store which node duplicates which
				NodeType* cur = NULL;
				std::vector<unsigned32> replacements(levelNodeCount);
				size_t uniqueNodes = 0;
				for (unsigned32 i = levelStartIndex; i < levelEndIndex; i++)
				{
					NodeType* node = GetTypedNode(i);
					if (cur == NULL || !node->Equals(*cur))
					{
						uniqueNodes++;
						cur = node;
					}
					replacements[node->GetIndex() - levelStartIndex] = cur->GetIndex();
				}
				if (verbose) printf(".");

				// Point all parents of nodes to the new replacement node
				// Note that this is only necessary if some nodes are replaced by others
				if (uniqueNodes < levelNodeCount)
				{
					tbb::parallel_for(parentLevelStartIndex, levelStartIndex, [&](const unsigned32 i)
					{
						Node* parent = GetTypedNode(i);
						unsigned32* children = parent->GetChildren();
						unsigned8 childCount = parent->GetChildCount();
						for (ChildIndex c = 0; c < childCount; c++)
							children[c] = replacements[children[c] - levelStartIndex];
					});
					if (verbose) printf(".");
					for (unsigned32 i = levelStartIndex; i < levelEndIndex; i++)
					{
						unsigned32 nodeIndex = GetTypedNode(i)->GetIndex();
						// If the node is not replaced by it's own index, then delete it
						if (replacements[nodeIndex - levelStartIndex] != nodeIndex)
						{
							Destroy(i);
							deletedCount++;
						}
					}
				}
				else if (verbose) printf(".");
			}
			else
			{
				if (verbose) printf(".");
				// For the leaf nodes (e.g. !fullRead), just add the first leaf node in the DAGNodePool, and replace the rest by it.
				NodeType* replacer = GetTypedNode(levelStartIndex);
				if (verbose) printf(".");
				if (levelNodeCount > 1)
				{
					tbb::parallel_for(parentLevelStartIndex, levelStartIndex, [&](const unsigned32 i)
					{
						NodeType* parent = GetTypedNode(i);
						unsigned32* children = parent->GetChildren();
						unsigned8 childCount = parent->GetChildCount();
						for (ChildIndex c = 0; c < childCount; c++)
							children[c] = levelStartIndex;
					});
					if (verbose) printf(".");
					for (unsigned32 i = levelStartIndex + 1; i < levelEndIndex; i++)
					{
						Destroy(i);
						deletedCount++;
					}
				}
				else if (verbose) printf(".");
			}
			if (verbose) printf(" Layer %2u compressed, %7u out of %7u nodes left\n", level, levelNodeCount - deletedCount, levelNodeCount);
		}
		Clean();
	}

	void ToOctree(unsigned8 fromLevel = 0)
	{
		auto levelIndices = SortOnLevel();
		BoolArray usedNodes(GetNodeCount(), false);
		unsigned32 newLevelStart = GetNodeCount();
		unsigned32 newLevelEnd = GetNodeCount();
		for (auto level = fromLevel; level < GetMaxLevel(); level++)
		{
			// Update start and end index of the nodes that were created during the last iteration
			newLevelStart = newLevelEnd;
			newLevelEnd = GetNodeCount();

			// Find indices of existing nodes for each level
			unsigned32 levelStart = levelIndices[level];
			unsigned32 levelEnd = levelIndices[level + 1];

			// Process all nodes
			for (unsigned32 i = levelStart; i < newLevelEnd; i++)
			{
				// Skip nodes of other levels
				if (i == levelEnd) i = newLevelStart;

				NodeType* node = GetTypedNode(i);

				unsigned32* children = node->GetChildren();
				unsigned8 childCount = node->GetChildCount();

				for (ChildIndex c = 0; c < childCount; c++)
				{
					if (!usedNodes[children[c]])
					{
						// If this node wasn't used yet, we can keep the same child.
						usedNodes.Set(children[c], true);
					}
					else
					{
						// If this node was used before, we need to create a new one
						auto oldNode = GetTypedNode(children[c]);
						auto newNode = Create(level + 1);
						newNode->SetChildren(oldNode->GetChildmask(), oldNode->GetChildren());
						newNode->CopyProperties(oldNode);
						children[c] = newNode->GetIndex();
					}
				}
			}
		}
	}

	void ClearOrphans()
	{
		BoolArray orphans(GetNodeCount(), true);
		orphans.Set(0, false);
		size_t orphansCleared = 0;
		bool firstPass = true;
		while (orphans.Any())
		{
			// Assume all nodes (except the root) are orphans
			orphans.SetRange(1, GetNodeCount(), true);
			for (unsigned32 i = 0; i < GetNodeCount(); i++)
			{
				NodeType* node = GetTypedNode(i);
				if (node == NULL) // Empty nodes are not orphans
					orphans.Set(i, false);
				else
				{
					// Roots are not orphans (by definition, they're just roots)
					if (node->GetLevel() == 0)
						orphans.Set(i, false);

					// All the nodes that are children of this node can not be orphans (since this node still lives)
					for (ChildIndex child = 0; child < node->GetChildCount(); child++)
						orphans.Set(node->GetChildren()[child], false);
				}
			}

			// Delete nodes that are orphans. Note that this might cause other nodes to be orphaned as well
			// In other words: if you're Batman's child, and Batman gets killed, then you are an orphan as well ;)
			for (unsigned32 i = 1; i < GetNodeCount(); i++)
				if (orphans.Get(i))
				{
					orphansCleared++;
					Destroy(i);
				}
		}

		printf("%llu orphans have been removed.\n", (unsigned64)orphansCleared);

		// Clean the nodepool to really remove all orphans
		Clean();
	}

	// Finds all parent of a node (O(N)).
	std::vector<std::pair<const NodeType*, ChildIndex>> FindParents(const NodeType* node) const
	{
		tbb::concurrent_queue<std::pair<const NodeType*, ChildIndex>> parents;
		unsigned32 childToFind = node->GetIndex();
		unsigned8 parentLevel = node->GetLevel() - 1;
		tbb::parallel_for((unsigned32)0, GetNodeCount(), [&](const unsigned32 i)
		{
			NodeType* node = GetTypedNode(i);
			if (node->GetLevel() == parentLevel)
			{
				for (ChildIndex child = 0; child < 8; child++)
					if (node->HasChild(child) && node->GetChildIndex(child) == childToFind)
						parents.push(std::pair<const NodeType*, ChildIndex>(node, child));
			}
		});
		std::vector<std::pair<const NodeType*, ChildIndex>> res;
		for (auto parent = parents.unsafe_begin(); parent != parents.unsafe_end(); parent++)
			res.push_back(*parent);
		return res;
	}

	// Cuts off all nodes that have a level higher than depth.
	void Shave(unsigned8 depth) override
	{
		if (depth >= GetMaxLevel()) return;

		auto levelIndices = SortOnLevel();

		// Make sure all nodes at level "depth" have no children (since we're gonna shave them off)
		const unsigned* emptyChildren = new unsigned[0];
		tbb::parallel_for(levelIndices[depth], levelIndices[depth + 1], [&](const unsigned32 i)
		{
			GetTypedNode(i)->SetChildren(ChildMask(0), emptyChildren);
		});
		delete emptyChildren;

		// Delete all nodes that have a higher level than depth
		for (unsigned32 i = levelIndices[depth + 1]; i < GetNodeCount(); i++)
			Destroy(i);

		// Resize the node pool so that no reference to the destroyed nodes exist
		mNodePool.Clean();
		mMaxLevel = depth;
	}

	virtual void PrintDebugInfo() const
	{
		printf("NodeCount: %u\n", GetNodeCount());
		unsigned64 leafVoxelCount = GetLeafVoxelCount();
		printf("Leaf voxel count: %llu\n", leafVoxelCount);
	}


protected:
	void Destroy(unsigned32 index) override { mNodePool.Delete(index); }
	virtual void Destroy(NodeType* node)
	{
		// Assert that we are actually deleting the correct node
		if (node->GetIndex() < GetNodeCount() && GetTypedNode(node->GetIndex()) == node)
			mNodePool.Delete(node->GetIndex());
		else
			printf("Error: Can't delete, node with ID %u not located at that position", node->GetIndex());
	}

	virtual void				InitializeReadTree() {}
	virtual void				FinalizeReadTree() {}

	virtual void				AppendPreProcess(glm::uvec3 coordinates, unsigned8 level, Tree* tree) {}
	virtual void				AppendPostProcess(glm::uvec3 coordinates, unsigned8 level, Tree* tree) {}
	
	// Sorts a subset of the nodes, but DOES NOT update the indices! Make sure to call UpdateNodeIndices at some point!
	template<typename Comparer>
	void SortBetween(unsigned32 startIndex, unsigned32 endIndex, const Comparer& comparer = NodeComparer())
	{
		if (startIndex >= endIndex || startIndex >= GetNodeCount()) return; // Nothing to sort

		mNodePool.Sort(startIndex, endIndex, comparer);

		// Check if the given range is monotonically increasing, to make sure the ndoes are still sorted on level (if they were before)
		if (mSortedOnLevel)
		{
			unsigned8 wantedLevel = GetTypedNode(startIndex)->GetLevel();
			for (unsigned32 i = startIndex; i < endIndex; i++)
			{
				unsigned8 level = GetTypedNode(i)->GetLevel();
				if (level != wantedLevel)
				{
					if (level > wantedLevel) wantedLevel = level;
					else
					{
						mSortedOnLevel = false;
						break;
					}
				}
			}
		}
	}

	//std::vector<NodeType*>		GetNodePoolCopy() const
	//{
	//	std::vector<NodeType*> nodePoolCopy(mNodePool.size());
	//	std::copy(mNodePool.begin(), mNodePool.end(), nodePoolCopy.begin());
	//	return nodePoolCopy;
	//}

	// Updates the current index value of all nodes to the value prescribed by the current nodepool.
	// oldMaxIndex is needed to know the size of the map that maps old to new indices. 
	// If oldMaxIndex is not provided or 0, it will be calculated, requiring an additional pass
	// through the nodes
	void UpdateNodeIndices(unsigned32 oldMaxIndex = 0)
	{
		if (oldMaxIndex == 0)
		{
			// oldMaxIndex = max(oldIndices)
			for (unsigned32 i = 0; i < GetNodeCount(); i++)
				if (GetTypedNode(i)->GetIndex() > oldMaxIndex)
					oldMaxIndex = GetTypedNode(i)->GetIndex();
			oldMaxIndex++;
		}
		// Update the indices in all nodes, store a map of where nodes have been moved
		std::vector<unsigned32> indexMap(oldMaxIndex + 1);
		tbb::parallel_for((unsigned32)0, GetNodeCount(), [&](const unsigned32 i) 
		{
			NodeType* node = GetTypedNode(i);
			if (node != NULL)
			{
				indexMap[node->GetIndex()] = i;
				node->SetIndex(i);
			}
		});
		// Update the child indices based on the node movement map
		//tbb::parallel_for((unsigned32)0, GetNodeCount(), [&](const unsigned32 i)
		for (unsigned32 i = 0; i < GetNodeCount(); i++)
		{
			NodeType* node = GetTypedNode(i);
			if (node != NULL)
			{
				const unsigned8 nodeChildCount = node->GetChildCount();
				unsigned32* children = node->GetChildren();
				for (ChildIndex c = 0; c < nodeChildCount; c++)
					children[c] = indexMap[children[c]];
			}
		}//);

		UpdateLocalReferences(indexMap);
	}

	virtual void UpdateLocalReferences(const std::vector<unsigned32>& indexMap)
	{
		if (mLeafsAreEqual)
			mLeafNode = indexMap[mLeafNode];
	}

	struct NodeComparer
	{
		bool operator()(const NodeType& a, const NodeType& b) const
		{
			return a.Compare(b);
		}
	};

	virtual	void				WriteAdditionalPoolProperties(std::ostream& file) {}
	virtual void				ReadAdditionalPoolProperties(std::istream& file) {}

	unsigned32					mLeafNode;
	bool						mLeafsAreEqual;
	
	unsigned8					mMaxLevel; // size of the texture in which the node pool will be stored
private:
	ObjectPool<NodeType>		mNodePool; // the node pool containing all nodes
	bool						mSortedOnLevel;
	//ObjectPool<NodeType>*		mNodeObjectPool; // The Node ObjectPool, used to reuse nodes instead of new and delete all the time
};