#ifndef OBSTACLETREE_H
#define OBSTACLETREE_H

#include "../../../geo/Sphere3.h"
#include "Obstacle3.h"
#include <algorithm>

struct ObstacleNode {
	bool isLeaf = true;
	Sphere3 boundSphere;
	std::vector<ObstacleNode*> sub;
	ObstacleNode(bool isLeaf = false) : isLeaf(isLeaf) {;}
};

struct ObstacleLeaf : public ObstacleNode {
	Obstacle3D obs;
	ObstacleLeaf() : ObstacleNode(true) {;}
};



class ObstacleTree {

	ObstacleNode root;

public:

	/** append a new leaf */
	void add(ObstacleLeaf* leaf) {
		root.sub.push_back(leaf);
	}

	void optimize() {
		while(true) {
			bool did = concat();
			if (!did) {break;}
		}
	}

	std::vector<Obstacle3D*> getHits(const Ray3 ray) const {
		std::vector<Obstacle3D*> obs;
		getHits(ray, &root, obs);
		return obs;
	}

	void getHits(const Ray3 ray, const ObstacleNode* node, std::vector<Obstacle3D*>& hits) const {
		for (const ObstacleNode* sub : node->sub) {
			if (sub->boundSphere.intersects(ray)) {
				if (sub->isLeaf) {
					ObstacleLeaf* leaf = (ObstacleLeaf*)(sub);
					hits.push_back(&leaf->obs);
				} else {
					if (sub->boundSphere.intersects(ray)) {getHits(ray, sub, hits);}
				}
			}
		}
	}


	bool concat() {

		bool concated = false;

		// first, sort all elements by radius (smallest first)
		auto compRadius = [] (const ObstacleNode* l1, const ObstacleNode* l2) {
			return l1->boundSphere.radius < l2->boundSphere.radius;
		};
		std::sort(root.sub.begin(), root.sub.end(), compRadius);

		ObstacleNode newRoot;

		// combine nearby elements
		//for (size_t i = 0; i < root.sub.size(); ++i) {
		while(true) {

			// get [and remove] the next element
			ObstacleLeaf* l0 = (ObstacleLeaf*) root.sub[0];
			root.sub.erase(root.sub.begin()+0);

			// find another element that yields minimal increase in volume
			auto compNear = [l0] (const ObstacleNode* l1, const ObstacleNode* l2) {
				const float d1 = Sphere3::join(l0->boundSphere, l1->boundSphere).radius;
				const float d2 = Sphere3::join(l0->boundSphere, l2->boundSphere).radius;
				return d1 < d2;
			};
			auto it = std::min_element(root.sub.begin(), root.sub.end(), compNear);
			ObstacleNode* l1 = *it;

			float increment = Sphere3::join(l0->boundSphere, l1->boundSphere).radius / l0->boundSphere.radius;

			const bool combine = (root.sub.size() > 1) && (it != root.sub.end()) && (increment < 1.75);

			if (combine) {

				// combine both into a new node
				ObstacleNode* node = new ObstacleNode();
				node->sub.push_back(l0);
				node->sub.push_back(*it);
				node->boundSphere = Sphere3::join(l0->boundSphere, (*it)->boundSphere);
				root.sub.erase(it);
				newRoot.sub.push_back(node);

				concated = true;

			} else {

				ObstacleNode* node = new ObstacleNode();
				node->sub.push_back(l0);
				node->boundSphere = l0->boundSphere;
				newRoot.sub.push_back(node);

			}

			// done?
			if (root.sub.size() == 1) {

				ObstacleNode* node = new ObstacleNode();
				node->sub.push_back(root.sub.front());
				node->boundSphere = root.sub.front()->boundSphere;
				newRoot.sub.push_back(node);
				break;

			} else if (root.sub.size() == 0) {
				break;
			}

			//--i;

		}

		root = newRoot;

		return concated;

	}

};

#endif // OBSTACLETREE_H