#ifndef INDOOR_GW3_REACHABLE_H
#define INDOOR_GW3_REACHABLE_H

#include <vector>
#include <set>
#include "../../Grid.h"

namespace GW3 {

#define likely(x)       __builtin_expect((x),1)
#define unlikely(x)     __builtin_expect((x),0)



/**
 * get all grid nodes that are reachable within x-edges (depth)
 */
template <typename Node> class ReachableByDepthUnsorted {

	struct VisitEntry {
		const Node* gn;
		int depth;
		VisitEntry() {;}
		VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;}
	};

	struct Visits {
		VisitEntry visits[512];// __attribute__((aligned(16)));
		size_t head = 0;
		size_t tail = 0;
		VisitEntry& getNext() {
			return visits[tail++];
		}
		void add(const VisitEntry& e) {
			visits[head++] = e;
			assert(head < 512);
			//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
		}
		bool hasMore() const {
			return head > tail;
		}
	};

	const Grid<Node>& grid;

public:

	ReachableByDepthUnsorted(const Grid<Node>& grid) : grid(grid) {
		;
	}

	/** get all nodes reachable from start using maxDepth steps */
	std::unordered_set<const Node*> get(const Node& start, const int maxDepth) {

		std::unordered_set<const Node*> checked;

		// assuming max 8 neighbors per node, we need
		// we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth)
		// which is 1 + (1+2+3+4+5)*neighbors
		// which is 1 + (n*n + n)/2*neighbors
		// however this seems to be slow?!
		//const int n = maxDepth + 1;
		//const int maxEntries = (n * n + n) / 2 * 10 + 1;
		//const int toAlloc = 4096 / sizeof(VisitEntry);
		//if ( unlikely(toAlloc < maxEntries) ) {return checked;}
		//if (maxDepth > 9) {throw Exception("will not fit!");}

		Visits toVisit;

		// directly start with the node itself and all its neighbors
		checked.insert(&start);
		for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
			const int nIdx = start.getNeighborIdx(i);
			const Node& gnNext = grid[nIdx];
			checked.insert(&gnNext);
			toVisit.add(VisitEntry(&gnNext, 1));
		}

		// check all to-be-visited nodes
		while ( likely(toVisit.hasMore()) ) {

			const VisitEntry& e = toVisit.getNext();

			if ( likely(e.depth <= maxDepth) ) {

				const Node* gnCur = e.gn;
				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
					const int nIdx = gnCur->getNeighborIdx(i);
					const Node& gnNext = grid[nIdx];
					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
						toVisit.add(VisitEntry(&gnNext, e.depth+1));
						checked.insert(&gnNext);
					}
				}

			}

		}

		return checked;

	}

};


/**
 * get all grid nodes that are reachable within x-edges (depth)
 * additionally returns the needed walking distance in meter
 */
template <typename Node> class ReachableByDepthWithDistanceSorted {

	struct VisitEntry {
		const Node* gn;
		int depth;
		float dist_m;
		int myIdx;
		VisitEntry() {;}
		VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) :
			gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;}
	};

	struct Visits {
		VisitEntry visits[1024];// __attribute__((aligned(16)));
		size_t head = 0;
		size_t tail = 0;
		VisitEntry& getNext() {
			return visits[tail++];
		}
		void add(const VisitEntry& e) {
			visits[head++] = e;
			assert(head < 1024);
			//if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?!
		}
		bool hasMore() const {
			return head > tail;
		}
		void sort() {
			const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
				return e1.dist_m < e2.dist_m;
			};
			std::sort(&visits[tail], &visits[head], comp);
		}
	};

	const Grid<Node>& grid;

public:

	/** result */
	struct Entry {

		const Node* node;
		const float walkDistToStart_m;
		const int prevIdx;

		Entry(const Node* node, const float dist, const size_t prevIdx) :
			node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;}

		bool hasPrev() const {
			return prevIdx >= 0;
		}

	};

	ReachableByDepthWithDistanceSorted(const Grid<Node>& grid) : grid(grid) {
		;
	}

	/** get all nodes reachable from start using maxDepth steps */
	std::vector<Entry> get(const Node& start, const int maxDepth) {

		std::unordered_set<const Node*> checked;
		std::vector<Entry> res;

		Visits toVisit;

		// directly start with the node itself and all its neighbors
		checked.insert(&start);
		res.push_back(Entry(&start, 0, -1));
		for (int i = 0; likely(i < start.getNumNeighbors()); ++i) {
			const int nIdx = start.getNeighborIdx(i);
			const Node& gnNext = grid[nIdx];
			const float dist_m = gnNext.getDistanceInMeter(start);
			toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size()));
			res.push_back(Entry(&gnNext, dist_m, 0));
			checked.insert(&gnNext);
		}
		toVisit.sort();

		// check all to-be-visited nodes
		while ( likely(toVisit.hasMore()) ) {

			const VisitEntry& e = toVisit.getNext();

			if ( likely(e.depth <= maxDepth) ) {

				const Node* gnCur = e.gn;

//				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
//					const int nIdx = gnCur->getNeighborIdx(i);
//					const Node& gnNext = grid[nIdx];
//					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
//						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
//						const float dist_m = e.dist_m + nodeNodeDist_m;
//						toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
//						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
//						checked.insert(&gnNext);
//					}
//				}

//				const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01;

				std::vector<VisitEntry> sub;

				for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) {
					const int nIdx = gnCur->getNeighborIdx(i);
					const Node& gnNext = grid[nIdx];
					if ( unlikely(checked.find(&gnNext) == checked.end()) ) {
						const float nodeNodeDist_m =  gnCur->getDistanceInMeter(gnNext);
						const float dist_m = e.dist_m + nodeNodeDist_m;
						//toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
						sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size()));
						res.push_back(Entry(&gnNext, dist_m, e.myIdx));
						checked.insert(&gnNext);

					}
				}

				// dijkstra.. sort the new nodes by destination to start
				// only sorting the 8 new nodes seems enough due to the graph's layout
				const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) {
					return e1.dist_m < e2.dist_m;
				};

				std::sort(sub.begin(), sub.end(), comp);

				for (const VisitEntry& e : sub) {
					toVisit.add(e);
				}

			}

			// slower with same result ;)
			//toVisit.sort();

		}

		return res;

	}

};

}

#endif // REACHABLE_H