#ifndef GRIDWALK3HELPER_H
#define GRIDWALK3HELPER_H

#include "../../../nav/dijkstra/Dijkstra.h"
#include <set>



template <typename Node> class GridWalk3Helper {

public:

	/** one walk along several nodes */
	struct Walk3 : public std::vector<const Node*> {

	};

	struct Walks3 : std::vector<Walk3> {

	};

	struct Nodes3 : std::vector<const Node*> {

	};

	/** get all possible walks from start within a given region */
	static Walks3 getAllPossibleWalks(Grid<Node>& grid, const Node* start, const float dist_m) {

		struct Access {
			Grid<Node>& grid;
			Access(Grid<Node>& grid) : grid(grid) {;}
			int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
			Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
			float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());}
		} acc(grid);

		const float addDist_m = grid.getGridSize_cm() / 100.0f;
		const float maxDist_m = dist_m * 1.1 + addDist_m;
		Dijkstra<Node> dijkstra;
		dijkstra.build(start, nullptr, acc, maxDist_m);

		const std::unordered_map<const Node*, DijkstraNode<Node>*>& nodes = dijkstra.getNodes();

		Walks3 walks;

		for (const auto& it : nodes) {
			Walk3 walk;
			DijkstraNode<Node>* node = it.second;
			do {
				const Node* gridNode = node->element;
				walk.insert(walk.begin(), gridNode);		// push_front() as dijkstra is inverted
				node = node->previous;
			} while (node);
			walks.push_back(walk);
		}

		return walks;

	}

	/** get all reachable nodes that are within a given range */
	static Nodes3 getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {

		Nodes3 res;
		std::unordered_map<uint32_t, float> distances;
		std::vector<uint32_t> toVisit;

		toVisit.push_back(start->getIdx());
		distances[start->getIdx()] = 0.0f;

		while (!toVisit.empty()) {

			int curIdx = toVisit.front();
			toVisit.erase(toVisit.begin());

			const Node& curNode = grid[curIdx];
			const float curDistance = distances[curIdx];
			res.push_back(&curNode);				// remember for output

			if (curDistance <= dist_m) {
				for (int i = 0; i < curNode.getNumNeighbors(); ++i) {

					const int neighborIdx = curNode.getNeighborIdx(i);
					const Node& neighbor = grid[neighborIdx];
					const float addDist = neighbor.inMeter().getDistance(curNode.inMeter());
					const float totalDist = curDistance + addDist;

					// this is like in dijkstra. keep the smallest distance to reach a node:

					// not yet reached -> store distance
					if (distances.find(neighborIdx) == distances.end()) {
						toVisit.push_back(neighborIdx);
						distances[neighborIdx] = totalDist;

					// reached earlier but found shorter way
					} else {
						if (distances[neighborIdx] > totalDist) {
							distances[neighborIdx] = totalDist;
						}
					}

				}
			}

		}

		return res;

	}

};

#endif // GRIDWALK3HELPER_H