////////////////////////////////////////////////////////////////////////////////
// SPTree.cpp
// Implementation of a space partition tree designed to speed up spacial queries
////////////////////////////////////////////////////////////////////////////////

#include "BGBase.h"
#include "intersections.h"
#include "SPTree.h"

#include <vector>
#include <algorithm>
#include <iterator>

// SPTreeCell constructor
SPTreeCell::SPTreeCell()
: VirtualClass("SPTreeCell", SPTreeCellID),
  negCell(nullptr),
  posCell(nullptr)
{
}

// SPTreeCell destructor
SPTreeCell::~SPTreeCell()
{
	if (posCell != nullptr) {
		delete posCell;
		posCell = nullptr;
	}
	if(negCell != nullptr) {
		delete negCell;
		negCell = nullptr;
	}
}


// SPTree constructor
SPTree::SPTree(size_t maxObjectsPerCell)
: VirtualClass("SPTree", SPTreeID),
  mMaxObjectsPerCell(maxObjectsPerCell)
{
	mMainCell.negCell = nullptr;
	mMainCell.posCell = nullptr;

	if (mMaxObjectsPerCell < 3) {
		mMaxObjectsPerCell = 3;
	}
}

// SPTree destructor
SPTree::~SPTree()
{
	recursivelyDeleteCells(mMainCell.negCell);
	recursivelyDeleteCells(mMainCell.posCell);
}

// Add object
void SPTree::add(RenderableObject* obj)
{
	addRecursive(&mMainCell, obj);
}

// Update object
void SPTree::update(RenderableObject* obj)
{
	remove(obj);
	add(obj);
}

// Remove object
void SPTree::remove(RenderableObject* obj)
{
	removeRecursive(&mMainCell, obj);
}

void SPTree::recursivelyDeleteCells(SPTreeCell* cell)
{
	if (cell != nullptr) {
		recursivelyDeleteCells(cell->negCell);
		recursivelyDeleteCells(cell->posCell);
		cell->negCell = nullptr;
		cell->posCell = nullptr;
		delete cell;
	}
}

void SPTree::moveRecursive(SPTreeCell* cell, std::vector<RenderableObject*>& target)
{
	if (cell != nullptr) {
		std::copy(cell->negObjects.begin(), cell->negObjects.end(), std::back_inserter(target));
		std::copy(cell->posObjects.begin(), cell->posObjects.end(), std::back_inserter(target));
		std::copy(cell->undecidedObjects.begin(), cell->undecidedObjects.end(), std::back_inserter(target));
		moveRecursive(cell->negCell, target);
		moveRecursive(cell->posCell, target);
		delete cell;
	}
}

struct PairOfRenderableObjectAndPlaneDistance {
	RenderableObject* ro;
	double dist;
};

bool operator<(const PairOfRenderableObjectAndPlaneDistance& a, const PairOfRenderableObjectAndPlaneDistance& b)
{
	if (a.dist < b.dist) {
		return true;
	} else {
		return false;
	}
}

void SPTree::split(SPTreeCell* cell)
{
	if (cell == nullptr) {
		return;
	}

	// Add all objects in the childs to this cell recursively
	std::copy(cell->negObjects.begin(), cell->negObjects.end(), std::back_inserter(cell->undecidedObjects));
	std::copy(cell->posObjects.begin(), cell->posObjects.end(), std::back_inserter(cell->undecidedObjects));
	moveRecursive(cell->negCell, cell->undecidedObjects);
	moveRecursive(cell->posCell, cell->undecidedObjects);
	cell->negCell = nullptr;
	cell->posCell = nullptr;

	if (cell->undecidedObjects.size() < mMaxObjectsPerCell) {
		return;
	}

	// Compute the min, max of all contained objects
/*	Point3D min = cell->undecidedObjects[0]->getMinCoords();
	Point3D max = cell->undecidedObjects[0]->getMaxCoords();

	for (size_t i = 1; i < cell->undecidedObjects.size(); ++i) {
		RenderableObject* ro = cell->undecidedObjects[i];
		Point3D roMin = ro->getMinCoords();
		Point3D roMax = ro->getMaxCoords();
		min = MinCoords(min, roMin);
		max = MaxCoords(max, roMax);
	}

	// Use the difference between min/max as the new normal
	Point3D planeNormal = max-min;
	Point3D planeOrigin = 0.5*(max+min);
	if (!planeNormal.unitLength()) {
		return;
	}

	{
		// Sort all contained objects along the normal by their min or center
		Plane p(planeOrigin, planeNormal);
		std::vector<PairOfRenderableObjectAndPlaneDistance> objectsSortedByPlaneDistance;
		for (size_t i = 0; i < cell->undecidedObjects.size(); ++i) {
			RenderableObject* ro = cell->undecidedObjects[i];
			PairOfRenderableObjectAndPlaneDistance pair;
			pair.ro = ro;
			Point3D roMid = 0.5 * (ro->getMinCoords() + ro->getMaxCoords());
			pair.dist = p.halfSpaceTest(roMid);
			objectsSortedByPlaneDistance.push_back(pair);
		}
		std::sort(objectsSortedByPlaneDistance.begin(), objectsSortedByPlaneDistance.end());

		// Pick the plane origin so that the contained objects are best separated
		size_t midNo = (int)(0.5 * objectsSortedByPlaneDistance.size());
		RenderableObject* midRo = objectsSortedByPlaneDistance[midNo].ro;
		if (midNo+1 < objectsSortedByPlaneDistance.size()) {
			RenderableObject* midRo2 = objectsSortedByPlaneDistance[midNo+1].ro;
			planeOrigin = 0.5 * (midRo->getMinCoords() + midRo->getMaxCoords() + midRo2->getMinCoords() + midRo2->getMaxCoords());
		} else {
			planeOrigin = 0.5 * (midRo->getMinCoords() + midRo->getMaxCoords());
		}
	}

	// Then, separate the objects by the separating plane
	cell->plane = Plane(planeOrigin, planeNormal);
	auto iter = cell->undecidedObjects.begin();
	while(iter != cell->undecidedObjects.end()) {
		RenderableObject* ro = *iter;
		Point3D roMin = ro->getMinCoords();
		Point3D roMax = ro->getMaxCoords();
		auto roMinDist = cell->plane.halfSpaceTest(roMin);
		auto roMaxDist = cell->plane.halfSpaceTest(roMax);

		if (roMinDist <= 0 && roMaxDist <= 0) {
			cell->negObjects.push_back(ro);
			iter = cell->undecidedObjects.erase(iter);
		} else {
			if (roMinDist >= 0 && roMaxDist >= 0) {
				cell->posObjects.push_back(ro);
				iter = cell->undecidedObjects.erase(iter);
			} else {
				++iter;
			}
		}
	}

	// If the remaining containers are still too big, split them as well
	if (cell->negObjects.size() > mMaxObjectsPerCell) {
		cell->negCell = new SPTreeCell();
		std::move(cell->negObjects.begin(), cell->negObjects.end(), std::back_inserter(cell->negCell->undecidedObjects));
		split(cell->negCell);
	}
	if (cell->posObjects.size() > mMaxObjectsPerCell) {
		cell->posCell = new SPTreeCell();
		std::move(cell->posObjects.begin(), cell->posObjects.end(), std::back_inserter(cell->posCell->undecidedObjects));
		split(cell->posCell);
	}*/
}

void SPTree::addRecursive(SPTreeCell* cell, RenderableObject* obj)
{
/*	if (cell != nullptr && obj != nullptr) {
		if (cell->plane.n.length() > 0.5) {
			// Use plane test to figure out where obj belongs
			Point3D objMin = obj->getMinCoords();
			Point3D objMax = obj->getMaxCoords();

			double objMinV = cell->plane.halfSpaceTest(objMin);
			double objMaxV = cell->plane.halfSpaceTest(objMax);

			if (objMinV < 0 && objMaxV < 0) {
				if (cell->negCell != nullptr) {
					addRecursive(cell->negCell, obj);
				} else {
					cell->negObjects.push_back(obj);
				}
			} else {
				if (objMinV > 0 && objMaxV > 0) {
					if (cell->posCell != nullptr) {
						addRecursive(cell->posCell, obj);
					} else {
						cell->posObjects.push_back(obj);
					}
				} else {
					cell->undecidedObjects.push_back(obj);
				}
			}
		} else {
			cell->undecidedObjects.push_back(obj);
		}

		size_t objCount = cell->undecidedObjects.size() + cell->negObjects.size() + cell->posObjects.size();
		if (objCount >= mMaxObjectsPerCell) {
			split(cell);
		}
	}*/
}

void SPTree::removeRecursive(SPTreeCell* cell, RenderableObject* obj)
{
	// Simply check all containers recursively and remove all matches ...
	if (cell != nullptr) {
		std::remove(cell->undecidedObjects.begin(), cell->undecidedObjects.end(), obj);
		std::remove(cell->negObjects.begin(), cell->negObjects.end(), obj);
		std::remove(cell->posObjects.begin(), cell->posObjects.end(), obj);
		removeRecursive(cell->negCell, obj);
		removeRecursive(cell->posCell, obj);
	}

	// todo: remove cell if empty?
	// Check present of child cells: delete cells that are empty and have no child nodes
	// If a node does not have objects and only one child node, delete the direct child node and store a link to its child here instead ...
	// (without deleting the child of the child)
}

// If o1 and o2 collide, add o2 to colliders
void testForCollision(RenderableObject* o1, RenderableObject* o2, std::vector<RenderableObject*>& colliders)
{
	// todo ...
}

void SPTree::getCollidersRecursive(RenderableObject* obj, std::vector<RenderableObject*>& colliders, SPTreeCell* cell)
{
/*	if (cell != nullptr) {
		for (size_t i = 0; i < cell->undecidedObjects.size(); ++i) {
			RenderableObject* obj2 = cell->undecidedObjects[i];
			testForCollision(obj, obj2, colliders);
		}
		Point3D objMin = obj->getMinCoords();
		Point3D objMax = obj->getMaxCoords();
		double objMinV = cell->plane.halfSpaceTest(objMin);
		double objMaxV = cell->plane.halfSpaceTest(objMax);

		if (objMinV < 0 || objMaxV < 0) {
			for (size_t i = 0; i < cell->negObjects.size(); ++i) {
				RenderableObject* obj2 = cell->negObjects[i];
				testForCollision(obj, obj2, colliders);
			}
			getCollidersRecursive(obj, colliders, cell->negCell);
		}
		if (objMinV < 0 || objMaxV < 0) {
			for (size_t i = 0; i < cell->negObjects.size(); ++i) {
				RenderableObject* obj2 = cell->posObjects[i];
				testForCollision(obj, obj2, colliders);
			}
			getCollidersRecursive(obj, colliders, cell->posCell);
		}
	}*/
}

// Check which other objects collide with obj
size_t SPTree::getColliders(RenderableObject* obj, std::vector<RenderableObject*>& colliders)
{
	getCollidersRecursive(obj, colliders, &mMainCell);
	return 0;
}

// Import scene from obj
void SPTree::importScene(RenderableObject* obj)
{
	for (size_t i = 0; i < obj->getObjectCount(); ++i) {
		RenderableObject* ro = obj->getObject(i);
		mMainCell.undecidedObjects.push_back(ro);
	}

	split(&mMainCell);
}
