#include "dxstdafx.h"
#include "mergetree.h"

MergeTree::MergeTree(unsigned short nOccluders, Occluder* occArray)
	:freeNodes( 4 )//nOccluders  >>1)
{
	maxR = 0.0f;
	this->nOccluders = nOccluders;
	locations = new unsigned short[nOccluders];
	pool = new Node[nOccluders << 1];
	nAllocated = 0;
	occluders = occArray;
}

MergeTree::~MergeTree(void)
{
	delete pool;
}


void MergeTree::searchOptimalMerger(unsigned short p)
{
	if(pool[p].type == MERGETREELEAFNODE)
	{
		if(indexOfCandidate == pool[p].occluderIndex)
		{
			foundMyselfAllRight = true;
			return;
		}
		float R = occluders[indexOfCandidate].solidRadius;
		float r = occluders[pool[p].occluderIndex].solidRadius;
        Vector diff = occluders[pool[p].occluderIndex].centre - occluders[indexOfCandidate].centre;
		float d = diff.norm();
		float cost;
		if(R > r)
			cost = d;// + r - R;
		else
			cost = d;// + R - r;
		if(cost < minCost){
			minCost = cost;
			minMerger = pool[p].occluderIndex;
		}
		return;
	}
	else
	{
		float distToCut = occluders[indexOfCandidate].centre[pool[p].type] - pool[p].cut;
		if(distToCut > 0.0f)
		{
			searchOptimalMerger( pool[p].right );
			if(distToCut < minCost /*+ maxR*/)
				searchOptimalMerger( pool[p].left );
		}
		else
		{
			searchOptimalMerger( pool[p].left );
			if(-distToCut < minCost /*+ maxR*/)
				searchOptimalMerger( pool[p].right );
		}
	}
}

void MergeTree::insert(unsigned short index)
{
	if(occluders[index].solidRadius > maxR)
		maxR = occluders[index].solidRadius;
	if(nAllocated == 0)
	{
		locations[index] = 0;
		pool[0].occluderIndex = index;
		pool[0].type = MERGETREELEAFNODE;
		pool[0].parent = 0;
		nAllocated++;
	}
	else
	{
		unsigned short p = 0;
		for(;;)
		{
			if(pool[p].type == MERGETREELEAFNODE)
			{
				unsigned short other = pool[p].occluderIndex;
				if(freeNodes.isEmpty())
				{
					pool[p].left = nAllocated++;
					pool[p].right = nAllocated++;
				}
				else
				{
					pool[p].left = freeNodes.removeMin();
					pool[p].right = freeNodes.removeMin();
				}

				Vector diff = occluders[index].centre - occluders[other].centre;

				char dominantDir;
				if(fabsf(diff[0]) > fabsf(diff[1]))
					if(fabsf(diff[0]) > fabsf(diff[2]))
						dominantDir = 0;
					else
						dominantDir = 2;
				else
					if(fabsf(diff[1]) > fabsf(diff[2]))
						dominantDir = 1;
					else
						dominantDir = 2;

				pool[p].type = dominantDir;
				pool[p].cut = occluders[other].centre[dominantDir] + (diff[dominantDir] * 0.5f);
				pool[pool[p].left].type = MERGETREELEAFNODE;
				pool[pool[p].left].occluderIndex = (diff[dominantDir] > 0.0f)?other:index;
				pool[pool[p].right].type = MERGETREELEAFNODE;
				pool[pool[p].right].occluderIndex = (diff[dominantDir] > 0.0f)?index:other;
				locations[other] = (diff[dominantDir] > 0.0f)?pool[p].left:pool[p].right;
				locations[index] = (diff[dominantDir] > 0.0f)?pool[p].right:pool[p].left;
				pool[locations[other]].parent = p;
				pool[locations[index]].parent = p;
				return;
			}
			else
			{
				if(occluders[index].centre[pool[p].type] > pool[p].cut)
					p = pool[p].right;
				else
					p = pool[p].left;
			}
		}
	}
}

void MergeTree::remove(unsigned short index)
{
	unsigned short p = locations[index];
	unsigned short parent = pool[p].parent;
	unsigned short grandpa = pool[parent].parent;
	freeNodes.insert(pool[parent].left);
	freeNodes.insert(pool[parent].right);
	if(pool[parent].left == p)
		pool[parent] = pool[pool[parent].right];
	else
		pool[parent] = pool[pool[parent].left];
	pool[parent].parent = grandpa;
	if(pool[parent].type == MERGETREELEAFNODE)
		locations[pool[parent].occluderIndex] = parent;
	else
	{
		pool[pool[parent].left].parent = parent;
		pool[pool[parent].right].parent = parent;
	}
}

bool MergeTree::verify(unsigned short p, float minx, float maxx, float miny, float maxy, float minz, float maxz)
{
	if(pool[p].type == MERGETREELEAFNODE)
	{
		Vector& a = occluders[pool[p].occluderIndex].centre;
		return a.x >= minx && a.x <= maxx &&
				a.y >= miny && a.y <= maxy &&
				a.z >= minz && a.z <= maxz;
	}
	else
	{
		if(pool[p].type == 0)
			return
				verify(pool[p].left, minx, pool[p].cut, miny, maxy, minz, maxz)
				&&
				verify(pool[p].right, pool[p].cut, maxx, miny, maxy, minz, maxz);
		if(pool[p].type == 1)
			return
				verify(pool[p].left, minx, maxx , miny, pool[p].cut, minz, maxz)
				&&
				verify(pool[p].right, minx, maxx, pool[p].cut, maxy, minz, maxz);
		if(pool[p].type == 2)
			return
				verify(pool[p].left, minx, maxx , miny, maxy, minz, pool[p].cut)
				&&
				verify(pool[p].right, minx, maxx, miny, maxy, pool[p].cut, maxz);
	}
}