#ifndef BOUNDINGVOLUMEHIERARCHY_H
#define BOUNDINGVOLUMEHIERARCHY_H

#include <vector>
#include <functional>

#include "BoundingVolume.h"
#include "BoundingVolumeAABB.h"
#include "BoundingVolumeSphere.h"




template <typename Element, typename Volume, typename Wrapper> class BVH {

protected:

	/** one node within the tree */
	struct BVHNode {
		bool isLeaf = true;
		Volume boundingVolume;
		std::vector<BVHNode*> childNodes;
		BVHNode(bool isLeaf = false) : isLeaf(isLeaf) {;}
	};

	/** one leaf within the tree */
	struct BVHLeaf : public BVHNode {
		Element element;
		BVHLeaf(const Element& e) : BVHNode(true), element(e) {;}
	};

	/** the tree's root */
	BVHNode root;

public:

	/** get the tree's root node */
	const BVHNode& getRoot() const {
		return root;
	}

	/** add a new volume to the tree */
	void add(const Element& element) {

		// create a new leaf for this element
		BVHLeaf* leaf = new BVHLeaf(element);

		// get the element's boundin volume
		leaf->boundingVolume = getBoundingVolume(element);

		// add the leaf to the tree
		root.childNodes.push_back(leaf);

	}

	/** optimize the tree */
	int optimize(const int max = 9999) {
		for (int i = 0; i < max; ++i) {
			//const bool did = concat();		// faster
			const bool did = combineBest();		// better
			if (!did) {return i;}
		}
		return max;
	}

	void getHits(const Ray3 ray, std::function<void(const Element&)> func) const {
		//int tests = 0; int leafs = 0;
		getHits(ray, &root, func);
		//std::cout << tests << " " << leafs << std::endl;
	}

	void getHits(const Ray3 ray, const BVHNode* node, std::function<void(const Element&)> func) const {
		for (const BVHNode* sub : node->childNodes) {
			if (sub->boundingVolume.intersects(ray)) {
				if (sub->isLeaf) {
					BVHLeaf* leaf = (BVHLeaf*)(sub);	// TODO: cast
					func(leaf->element);
				} else {
					getHits(ray, sub, func);
				}
			}
		}
	}

private:

	bool combineBest() {

		// nothing to do?
		if (root.childNodes.size() < 2) {return false;}

		struct Best {
			BVHNode* n1 = nullptr;
			BVHNode* n2 = nullptr;
			Volume vol;
			float volSize = 99999999;
		} best;

		for (size_t i = 0; i < root.childNodes.size(); ++i) {
			for (size_t j = 0; j < root.childNodes.size(); ++j) {

				if (i == j) {continue;}

				BVHNode* n1 = root.childNodes[i];
				BVHNode* n2 = root.childNodes[j];

				const Volume newVol = Volume::join(n1->boundingVolume, n2->boundingVolume);
				const float newVolSize = newVol.getVolumeSize();
				if (newVolSize < best.volSize) {
					best.vol = newVol;
					best.volSize = newVolSize;
					best.n1 = n1;
					best.n2 = n2;
				}

			}
		}

		root.childNodes.erase(std::remove(root.childNodes.begin(), root.childNodes.end(), best.n1), root.childNodes.end());
		root.childNodes.erase(std::remove(root.childNodes.begin(), root.childNodes.end(), best.n2), root.childNodes.end());

		// combine both into a new node
		BVHNode* newNode = new BVHNode();
		newNode->childNodes.push_back(best.n1);
		newNode->childNodes.push_back(best.n2);
		newNode->boundingVolume = best.vol;

		// does the newly created node contain any other nodes?
		// THIS SHOULD NEVER BE THE CASE!
//		for (size_t i = 0; i < root.childNodes.size(); ++i) {
//			BVHNode* n3 = root.childNodes[i];
//			if (newNode->boundingVolume.contains(n3->boundingVolume)) {
//				newNode->childNodes.push_back(n3);
//				root.childNodes.erase(root.childNodes.begin()+i);
//				--i;
//			}
//		}

		// attach the node
		root.childNodes.push_back(newNode);

		return true;

	}


	bool concat() {

		// nothing to do?
		if (root.childNodes.size() < 2) {return false;}


		bool concated = false;

		// first, sort all elements by volume (smallest first)
		auto compVolume = [] (const BVHNode* n1, const BVHNode* n2) {
			return n1->boundingVolume.getVolumeSize() < n2->boundingVolume.getVolumeSize();
		};
		std::sort(root.childNodes.begin(), root.childNodes.end(), compVolume);


		// elements will be grouped into this new root
		BVHNode newRoot;

		// combine nearby elements
		while(true) {

			// get [and remove] the next element
			BVHNode* n0 = (BVHNode*) root.childNodes[0];
			root.childNodes.erase(root.childNodes.begin()+0);

			// find another element that yields minimal increase in volume
			auto compNear = [n0] (const BVHNode* n1, const BVHNode* n2) {
				const float v1 = Volume::join(n0->boundingVolume, n1->boundingVolume).getVolumeSize();
				const float v2 = Volume::join(n0->boundingVolume, n2->boundingVolume).getVolumeSize();
				return v1 < v2;
			};
			auto it = std::min_element(root.childNodes.begin(), root.childNodes.end(), compNear);
			BVHNode* n1 = *it;

			// calculate the resulting increment in volume
			const Volume joined = Volume::join(n0->boundingVolume, n1->boundingVolume);
			const float increment = joined.getVolumeSize() / n0->boundingVolume.getVolumeSize();
			const bool intersects = n0->boundingVolume.intersects(n1->boundingVolume);

			const bool combine = true; //(intersects); //(increment < 15.0);

			if (combine) {

				// remove from current root
				root.childNodes.erase(it);

				// combine both into a new node
				BVHNode* node = new BVHNode();
				node->childNodes.push_back(n0);
				node->childNodes.push_back(n1);
				node->boundingVolume = joined;
				newRoot.childNodes.push_back(node);

				concated = true;

			} else {

				BVHNode* node = new BVHNode();
				node->childNodes.push_back(n0);
				node->boundingVolume = n0->boundingVolume;
				newRoot.childNodes.push_back(node);

			}

			// done?
			if (root.childNodes.size() == 1) {
				BVHNode* node = new BVHNode();
				node->childNodes.push_back(root.childNodes.front());
				node->boundingVolume = root.childNodes.front()->boundingVolume;
				newRoot.childNodes.push_back(node);
				break;
			} else if (root.childNodes.size() == 0) {
				break;
			}

		}

		root = newRoot;
		return concated;

	}

	/** get a bounding-volume for the given element */
	Volume getBoundingVolume(const Element& element) {
		const std::vector<Point3> verts = Wrapper::getVertices(element);
		return Volume::fromVertices(verts);
	}



};

#endif // BOUNDINGVOLUMEHIERARCHY_H