/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef INDOOR_GW3_HELPER_H
#define INDOOR_GW3_HELPER_H

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

#include "Structs.h"

#include <vector>
#include <set>

#include <KLib/math/random/RandomIterator.h>

//#define SHOW_DEBUG_PLOT

#ifdef SHOW_DEBUG_PLOT
	#include <KLib/misc/gnuplot/Gnuplot.h>
	#include <KLib/misc/gnuplot/GnuplotPlot.h>
	#include <KLib/misc/gnuplot/GnuplotPlotElementColorPoints.h>
#endif

namespace GW3 {


	/** get an iterator over all nodes reachable from the given start */
	template <typename Node> class ReachableIteratorSorted {

	const Grid<Node>& grid;
	const Node& start;

	struct Next {

		uint32_t idx;
		float distToStart;

		Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;}

		/** compare by weight. same weight? : compare by pointer */
		bool operator < (const Next& o) const	{
			return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx);
		}

	};

	Node* curNode = nullptr;
	std::unordered_set<uint32_t> visited;
	std::set<Next> toVisit;

	public:

		ReachableIteratorSorted(const Grid<Node>& grid, const Node& start) : grid(grid), start(start) {
			toVisit.insert(Next(start.getIdx(),0));
		}

		bool hasNext() const {
			return !toVisit.empty();
		}

		const Node& next() {

			const Next cur = *(toVisit.begin());				// visit from inside out (needed for correct distance)
			toVisit.erase(toVisit.begin());
			visited.insert(cur.idx);

			const Node& curNode = grid[cur.idx];

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

				const int neighborIdx = curNode.getNeighborIdx(i);
				const Node& neighbor = grid[neighborIdx];
				const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor);

				// not yet reached -> store distance
				if (visited.find(neighborIdx) == visited.end()) {
					toVisit.insert(Next(neighborIdx, newDist));
				}

			}

			// done
			return curNode;

		}

	};

	/**
	 * data-structure to track to-be-visited nodes
	 * push_back, pop_front
	 * as pop_front is costly, we omit the pop and use a head-index instead
	 * memory-consumption vs speed
	 */
	struct ToVisit {
		size_t nextIdx = 0;
		std::vector<uint32_t> vec;
		ToVisit() {vec.reserve(256);}
		void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);}
		uint32_t next() {return vec[nextIdx++];}
		bool empty() const {return nextIdx >= vec.size();}
	};

	/** get an iterator over all nodes reachable from the given start */
	template <typename Node, typename Conditions> class ReachableIteratorUnsorted {

	const Grid<Node>& grid;
	const Node& start;

	Node* curNode = nullptr;
	std::unordered_set<uint32_t> visited;
	ToVisit toVisit;

	Conditions cond;

	public:

		ReachableIteratorUnsorted(const Grid<Node>& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) {
			toVisit.add(start.getIdx());
		}

		bool hasNext() const {
			return !toVisit.empty();
		}

		//const Node& next(const std::function<bool(const Node&)>& skip) {
		//template <typename Skip> const Node& next(const Skip skip) {
		const Node& next() {

			// get the next to-be-visited node
			const uint32_t curIdx = toVisit.next();			//visit from inside out (needed for correct distance)
			const Node& curNode = grid[curIdx];

			// mark as "visited"
			visited.insert(curIdx);

			// get all neighbors
			const int numNeighbors = curNode.getNumNeighbors();
			for (int i = 0; i < numNeighbors; ++i) {

				const uint32_t neighborIdx = curNode.getNeighborIdx(i);
				const Node& neighbor = grid[neighborIdx];

				const bool visit = cond.visit(neighbor) ;

				// not yet reached -> store distance
				if (visit) {
					if (visited.find(neighborIdx) == visited.end()) {
						toVisit.add(neighborIdx);
					}
				}

			}

			// done
			return curNode;

		}

	};


	struct ReachableSettings {

		float dist_m;
		bool limitDistance = true;

		Heading heading = Heading(0);
		float maxHeadingDiff_rad;
		bool limitHeading = false;

	};


	template <typename Node> class Helper {

	public:

//		static GridPoint p3ToGp(const Grid<Node>& grid, const Point3 p) {
//			const Point3 p100 = p*100;
//			//return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) );
//			return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(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;
//		}

		/** does one of the given grid-nodes contains the provided point-in-question? */
		static const Node* contains(const Grid<Node>& grid, const std::vector<const 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 ReachableSettings& set ) {

			//auto tStart = std::chrono::system_clock::now();

			Nodes<Node> res;
			std::unordered_map<uint32_t, float> distances;
			std::vector<uint32_t> toVisit;						// std::queue was only barely faster: 900 vs 880 microseconds

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

			#ifdef SHOW_DEBUG_PLOT
				static K::Gnuplot gp;
				K::GnuplotPlot plot;
				K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1);
				plot.add(&pts1);
			#endif

			while (!toVisit.empty()) {

				const int curIdx = toVisit.front();				// visit from inside out (needed for correct distance)
				toVisit.erase(toVisit.begin());

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

				#ifdef SHOW_DEBUG_PLOT
					pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance);
					gp.draw(plot);
					gp.flush();
				#endif

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

					const int neighborIdx = curNode.getNeighborIdx(i);
					const Node& neighbor = grid[neighborIdx];
					const float addDist = neighbor.getDistanceInMeter(curNode);
					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()) {
						distances[neighborIdx] = totalDist;

						if (set.limitDistance) {
							if (totalDist > set.dist_m) {continue;}
						}
						if (set.limitHeading) {
							const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm);		// angle between start and current node
							const float diff = head.getDiffHalfRAD(set.heading);							// difference between above angle and requested angle
							if (diff > set.maxHeadingDiff_rad) {continue;}									// more than 90 degree difference? -> ignore
						}
						toVisit.push_back(neighborIdx);	// needs a visit? (still some distance left)


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

					}

				}

			}

			//auto tEnd = std::chrono::system_clock::now();
			//auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(tEnd - tStart);
			//std::cout << elapsed.count() << std::endl;

			return res;

		}

	};

}

#endif // INDOOR_GW3_HELPER_H