#ifndef NAV_MESH_H
#define NAV_MESH_H

#include "NavMeshTriangle.h"
#include <vector>
#include "../geo/BBox3.h"
#include <random>
#include "../math/DrawList.h"
#include "NavMeshRandom.h"
#include "NavMeshLocation.h"

namespace NM {

	template <typename Tria> class NavMesh {

		/** all triangles within the mesh */
		std::vector<Tria*> triangles;

		BBox3 bbox;

	public:

		/** ctor */
		NavMesh() {

		}

		/** dtor */
		~NavMesh() {
			for (const Tria* t : triangles) {delete t;}
			triangles.clear();
		}

		/** the overall bounding-box */
		const BBox3 getBBox() const {
			return bbox;
		}

		/** add a new triangle */
		void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) {
			triangles.push_back(new Tria(p1,p2,p3,type));
			bbox.add(p1);
			bbox.add(p2);
			bbox.add(p3);
		}

		/** get the triangle this point belongs to (if any) */
		NavMeshLocation<Tria> getLocation(const Point3 pos) const {
			for (const Tria* tria : triangles) {
				if (tria->contains(pos)) {
					return NavMeshLocation<Tria>(pos, tria);
				}
			}
			throw Exception("location not found within NavMesh: " + pos.asString());
		}


		/** get the triangle/point on the mesh that is nearest to the given location */
		NavMeshLocation<Tria> getLocationNearestTo(const Point3 pos) const {
			auto comp = [pos] (const Tria* t1, const Tria* t2) {
				//return t1->getCenter().getDistance(pos) < t2->getCenter().getDistance(pos);
				return t1->getDistanceApx(pos) < t2->getDistanceApx(pos);
			};
			auto it = std::min_element(triangles.begin(), triangles.end(), comp);
			const Tria* best = *it;
			Point3 rPos = best->toPoint3Near(pos.xy());
			return NavMeshLocation<Tria>(rPos, best);
		}

		/** connect both triangles */
		void connectBiDir(int idx1, int idx2) {
			connectUniDir(idx1,idx2);
			connectUniDir(idx2,idx1);
		}

		/** connect both triangles */
		void connectUniDir(int idxFrom, int idxTo) {
			Tria* tria = triangles[idxFrom];
			tria->addNeighbor(triangles[idxTo]);
		}

		/** allows for-each iteration over all included triangles */
		decltype(triangles.begin()) begin() {return triangles.begin();}

		/** allows for-each iteration over all included triangles */
		decltype(triangles.end()) end() {return triangles.end();}

		/** array access */
		Tria* operator [] (const size_t idx) {
			Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds");
			return triangles[idx];
		}

		/** get the number of triangles used */
		size_t getNumTriangles() const {
			return triangles.size();
		}

		/** ---------------- MISC ---------------- */


		/** get a random-generator for several mesh-actions */
		NavMeshRandom<Tria> getRandom() const {
			return NavMeshRandom<Tria>(triangles);
		}

	//	/** ---------------- NEIGHBORS ---------------- */

	//	/** get the number of neighbors for the given element */
	//	int getNumNeighbors(const size_t idx) const {
	//		return getNumNeighbors(triangles[idx]);
	//	}

	//	/** get the number of neighbors for the given element */
	//	int getNumNeighbors(const Tria& e) const {
	//		return e._numNeighbors;
	//	}

	//	/** get the n-th neighbor for the given node */
	//	Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const {
	//		const Tria& node = triangles[nodeIdx];
	//		return getNeighbor(node, nth);
	//	}

	//	/** get the n-th neighbor for the given node */
	//	Tria& getNeighbor(const Tria& tria, const size_t nth) const {
	//		const Tria& neighbor = triangles[tria._neighbors[nth]];
	//		return (Tria&) neighbor;
	//	}

	};

}

#endif