#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, int count)

{

	int axis;

	float pos;

	//if (stopcriterionmet()) return

	// 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 split;

	int rest;

	for (split = splitlist.begin(); split != splitlist.end(); split++)

	{

		for (sh = sslist.begin(), rest = sslist.size(); sh != sslist.end(); sh++, rest--)

		{

			if (sh->hasMark())

				continue;

			// if shape is on left side of split plane

			if (sh->bbox.R.cell[axis] <= split->pos)

			{

				sh->setMark();

				split->lnum++;

			}

			// if shape is completely contained in split plane

			if (sh->bbox.L.cell[axis] == sh->bbox.R.cell[axis] == split->pos)

			{

				if (split->pos - bbox.L.cell[axis] < bbox.R.cell[axis] - split->pos)

				{

					// left subcell is smaller -> if not empty, put shape here

					if (split->lnum)

						split->lnum++;

					else

						split->rnum++;

				} else {

					// right subcell is smaller

					if (split->rnum)

						split->rnum++;

					else

						split->lnum++;

				}

			}

			// if shape is on right side of split plane

			if (sh->bbox.L.cell[axis] >= split->pos)

			{

				split->rnum += rest;

				break;

			}

			// if shape occupies both sides of split plane

			if (sh->bbox.L.cell[axis] < split->pos && sh->bbox.R.cell[axis] > split->pos)

			{

				split->lnum++;

				split->rnum++;

			}

		}

	}

	// choose best split pos

	// ... //

	KdNode *children = new KdNode[2];

	ShapeList::iterator shape;

	for (shape = shapes.begin(); shape != shapes.end(); shape++)

	{

		if (((Triangle*)*shape)->A.cell[axis-1] < pos

		|| ((Triangle*)*shape)->B.cell[axis-1] < pos

		|| ((Triangle*)*shape)->C.cell[axis-1] < pos)

			children[0].addShape(*shape);

		if (((Triangle*)*shape)->A.cell[axis-1] >= pos

		|| ((Triangle*)*shape)->B.cell[axis-1] >= pos

		|| ((Triangle*)*shape)->C.cell[axis-1] >= pos)

			children[1].addShape(*shape);

	}

	bbox.R[axis-1] = pos;

	children[0].subdivide(bbox, depth+1, children[0].shapes.size());

	bbox.L[axis-1] = pos;

	children[1].subdivide(bbox, depth+1, children[1].shapes.size());

}

void KdTree::build()

{

	root = new KdNode();

	root->shapes = shapes;

	root->subdivide(bbox, 0, shapes.size());

}