#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"

template <typename Tria> class NavMesh {

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

	BBox3 bbox;

public:

	NavMesh() {

	}

	/** 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(Tria(p1,p2,p3,type));
		bbox.add(p1);
		bbox.add(p2);
		bbox.add(p3);
	}

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

	/** connect both triangles */
	void connectUniDir(int idxFrom, int idxTo) {
		NavMeshTriangle& tria = triangles[idxFrom];
		tria._neighbors[tria._numNeighbors] = 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 ---------------- */


	NavMeshRandom<Tria> getRandomizer() {
		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