#include <algorithm>

#include "kdtree.h"

void Container::addShape(Shape* aShape)
{
	shapes.push_back(aShape);
	if (shapes.size() == 0) {
		/* initialize bounding box */
		bbox = aShape->get_bbox();
	} else {
		/* adjust bounding box */
		BBox shapebb = aShape->get_bbox();
		if (shapebb.L.x < bbox.L.x)  bbox.L.x = shapebb.L.x;
		if (shapebb.L.y < bbox.L.y)  bbox.L.y = shapebb.L.y;
		if (shapebb.L.z < bbox.L.z)  bbox.L.z = shapebb.L.z;
		if (shapebb.R.x > bbox.R.x)  bbox.R.x = shapebb.R.x;
		if (shapebb.R.y > bbox.R.y)  bbox.R.y = shapebb.R.y;
		if (shapebb.R.z > bbox.R.z)  bbox.R.z = shapebb.R.z;
	}
};

void KdNode::subdivide(BBox bbox, int depth)
{
	// choose split axis
	axis = 0;
	if (bbox.R.y - bbox.L.y > bbox.R.x - bbox.L.x)
		axis = 1;
	if (bbox.R.z - bbox.L.z > bbox.R.y - bbox.L.y)
		axis = 2;

	// *** find optimal split position
	SortableShapeList sslist(shapes, axis);
	sort(sslist.begin(), sslist.end());

	SplitList splitlist = SplitList();

	SortableShapeList::iterator sh;
	for (sh = sslist.begin(); sh != sslist.end(); sh++)
	{
		splitlist.push_back(SplitPos(sh->bbox.L.cell[axis]));
		splitlist.push_back(SplitPos(sh->bbox.R.cell[axis]));
	}
	sort(splitlist.begin(), splitlist.end());

	// find all posible splits
	SplitList::iterator spl;
	int rest;
	for (spl = splitlist.begin(); spl != splitlist.end(); spl++)
	{
		for (sh = sslist.begin(), rest = sslist.size(); sh != sslist.end(); sh++, rest--)
		{
			if (sh->hasMark())
				continue;
			// if shape is completely contained in split plane
			if (spl->pos == sh->bbox.L.cell[axis] == sh->bbox.R.cell[axis])
			{
				if (spl->pos - bbox.L.cell[axis] < bbox.R.cell[axis] - spl->pos)
				{
					// left subcell is smaller -> if not empty, put shape here
					if (spl->lnum)
						spl->lnum++;
					else
						spl->rnum++;
				} else {
					// right subcell is smaller
					if (spl->rnum)
						spl->rnum++;
					else
						spl->lnum++;
				}
			} else
			// if shape is on left side of split plane
			if (sh->bbox.R.cell[axis] <= spl->pos)
			{
				sh->setMark();
				spl->lnum++;
			} else
			// if shape occupies both sides of split plane
			if (sh->bbox.L.cell[axis] < spl->pos && sh->bbox.R.cell[axis] > spl->pos)
			{
				spl->lnum++;
				spl->rnum++;
			} else
			// if shape is on right side of split plane
			if (sh->bbox.L.cell[axis] >= spl->pos)
			{
				spl->rnum += rest;
				break;
			}
		}
	}

	// choose best split pos
	const float K = 1.4; // constant, K = cost of traversal / cost of ray-triangle intersection
	float SAV = 2*(bbox.w()*bbox.h() + bbox.w()*bbox.d() + bbox.h()*bbox.d()); // surface area of node
	float cost = SAV * (K + shapes.size()); // initial cost = non-split cost
	SplitPos *splitpos = NULL;
	leaf = true;
	BBox lbb = bbox;
	BBox rbb = bbox;
	for (spl = splitlist.begin(); spl != splitlist.end(); spl++)
	{
		// calculate SAH cost of this split
		lbb.R.cell[axis] = spl->pos;
		rbb.L.cell[axis] = spl->pos;
		float SAL = 2*(lbb.w()*lbb.h() + lbb.w()*lbb.d() + lbb.h()*lbb.d());
        float SAR = 2*(rbb.w()*rbb.h() + rbb.w()*rbb.d() + rbb.h()*rbb.d());
        float splitcost = K + SAL/SAV*(K+spl->lnum) + SAR/SAV*(K+spl->rnum);

		if (splitcost < cost)
		{
			leaf = false;
			cost = splitcost;
			splitpos = &*spl;
		}
	}

	if (leaf)
		return;

	split = splitpos->pos;
	float lnum = splitpos->lnum;
	float rnum = splitpos->rnum;

	// split this node
	children = new KdNode[2];
	int state = 0;
	for (sh = sslist.begin(); sh != sslist.end(); sh++)
	{
		// if shape is on left side of split plane
		if (state == 1)
		{ // only right
			children[1].addShape(sh->shape);
			continue;
		}
		if (state == 0)
		{
			if (sh->bbox.R.cell[axis] < split)
			{ // left
				children[0].addShape(sh->shape);
			} else
			if (sh->bbox.R.cell[axis] > split)
			{
				if (sh->bbox.L.cell[axis] < split)
				{ // both
					children[0].addShape(sh->shape);
					children[1].addShape(sh->shape);
				} else
				{ // right
					children[1].addShape(sh->shape);
					state = 1;
				}
			} else
			{ // R == split
				if (sh->bbox.L.cell[axis] < split)
				{ // left
					children[0].addShape(sh->shape);
				} else
				{ // contained
					if (split - bbox.L.cell[axis] < bbox.R.cell[axis] - split)
					{
						// left subcell is smaller -> if not empty, put shape here
						if (lnum)
							children[0].addShape(sh->shape);
						else
							children[1].addShape(sh->shape);
					} else {
						// right subcell is smaller
						if (rnum)
							children[1].addShape(sh->shape);
						else
							children[0].addShape(sh->shape);
					}
				}
			}
		}
	}

	lbb.R.cell[axis] = split;
	rbb.L.cell[axis] = split;

	children[0].subdivide(lbb, depth+1);
	children[1].subdivide(rbb, depth+1);
}

void KdTree::build()
{
	root = new KdNode();
	root->shapes = shapes;
	root->subdivide(bbox, 0);
}