#ifndef NAVMESHSUB_H
#define NAVMESHSUB_H

#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../NavMeshRandom.h"

#include <vector>
#include <unordered_set>

namespace NM {

	template <typename Tria> class NavMeshSub {

		std::vector<const Tria*> toVisit;

	public:

		NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
			build(loc,radius_m);
		}

		/** does this submesh contain the given point? */
		bool contains(const Point2 p2) const {
			for (const Tria* t : toVisit) {
				if (t->contains(p2)) {return true;}
			}
			return false;
		}

		/** does this submesh contain the given point? */
		bool contains(const Point3 p3) const {
			for (const Tria* t : toVisit) {
				if (t->contains(p3)) {return true;}
			}
			return false;
		}

		/** get the triangle that contains the given point (if any) */
		const Tria* getContainingTriangle(const Point2 p2) const {
			for (const Tria* t : toVisit) {
				if (t->contains(p2)) {return t;}
			}
			return nullptr;
		}

		/** perform random operations on the submesh */
		NavMeshRandom<Tria> getRandom() const {
			return NavMeshRandom<Tria>(toVisit);
		}


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

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



	private:

		void build(const NavMeshLocation<Tria>& loc, float radius_m) {

			PERF_REGION(6, "NavMeshSub::build()");

			std::unordered_set<const Tria*> visited;

			// starting-triangle + all its (max 3) neighbors
			toVisit.push_back(loc.tria);
			visited.insert(loc.tria);
//			for (const auto* n : *loc.tria) {
//				toVisit.push_back( (const Tria*)n );
//			}
//			size_t next = 1;	// start with the first neighbor (skip starting triangle itself)

			size_t next = 0;
			while (next < toVisit.size()) {

				// next triangle
				const NavMeshTriangle* cur = toVisit[next]; ++next;

				// neighbors
				for (const auto* n : *cur) {
					const Tria* t = (const Tria*) n;
					//const float dist = loc.pos.getDistance(n->getCenter());
					const float dist = n->getDistanceApx(loc.pos);
					if (dist > radius_m) {continue;}
					if (visited.find(t) != visited.end()) {continue;}
					toVisit.push_back(t);
					visited.insert(t);
				}

			}

		}


	};

}

#endif // NAVMESHSUB_H