#ifndef INDOOR_GW3_HELPER_H
#define INDOOR_GW3_HELPER_H

#include "../../../nav/dijkstra/Dijkstra.h"
#include "../../Grid.h"

#include "Structs.h"

#include <set>

namespace GW3 {

	template <typename Node> class Helper {

	public:

		static GridPoint p3ToGp(const Point3 p) {
			const Point3 p100 = p*100;
			return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
		}

		static Point3 gpToP3(const GridPoint gp) {
			return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f);
		}

		/** does the given grid-node contain the provided point-in-question? */
		static bool contains(const Grid<Node>& grid, const Node* n, Point2 pt) {
			const float gridSize_m = grid.getGridSize_cm() / 100.0f;
			const float d = gridSize_m / 2.0f;
			const Point2 pMin = n->inMeter().xy() - Point2(d, d);
			const Point2 pMax = n->inMeter().xy() + Point2(d, d);
			const BBox2 bbox(pMin, pMax);
			return bbox.contains(pt);
		}

		/** does one of the given grid-nodes contains the provided point-in-question? */
		static const Node* contains(const Grid<Node>& grid, const Nodes<Node>& nodes, Point2 pt) {
			for (const Node* n : nodes) {
				if (contains(grid, n, pt)) {return n;}
			}
			return nullptr;
		}

		/** get all possible walks from start within a given region */
		static Walks<Node> 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();

			Walks<Node> walks;

			for (const auto& it : nodes) {
				Walk<Node> 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 Nodes<Node> getAllReachableNodes(Grid<Node>& grid, const Node* start, const float dist_m) {

			Nodes<Node> 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 // INDOOR_GW3_HELPER_H