#ifndef NAVMESHSUB_H
#define NAVMESHSUB_H

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

#include <vector>
#include <unordered_set>


template <typename Tria> class NavMeshSub {

	std::vector<const Tria*> toVisit;

public:

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

private:

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

		// center to start searching
		const Point3 center = loc.pos;

		toVisit.push_back(loc.tria);

		std::unordered_set<const Tria*> visited;

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

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

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

		}

		return toVisit;

	}


};

#endif // NAVMESHSUB_H