#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());
}