/*
 * © 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 GRID_H
#define GRID_H

#include <vector>
#include <iostream>
#include <unordered_map>
#include <algorithm>

#include "../Assertions.h"

#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"

#include "../Assertions.h"
#include "../geo/BBox3.h"
#include "../misc/Debug.h"

#define GM_BOX			1
#define GM_HOBEYCOMB	2
#define GRID_MODE		GM_BOX


/**
 * grid of a given-size, storing some user-data-value which
 * - extends GridPoint and GridNode
 *
 * Usage:
 *		for (Node& n : grid) {...}
 *		for (Node& n2 : grid.neighbors(n)) {...}
 *
 */
template <typename T> class Grid {

	static constexpr const char* name = "Grid";

	#include "GridNeighborIterator.h"

	/** UID for nodes */
	typedef uint64_t UID;

private:

	/** all elements (nodes) within the grid */
	std::vector<T> nodes;

	/** UID -> index mapping */
	std::unordered_map<UID, int> hashes;

	/** the user-given grid-size */
	const int gridSize_cm;

public:

	/** ctor with the grid's size (in cm) */
	Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) {
		//static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
		//static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
		StaticAssert::AinheritsB<T, GridNode>();		// "T must inherit from GridNode!"
		StaticAssert::AinheritsB<T, GridPoint>();		// "T must inherit from GridPoint!"
		Log::add(name, "empty grid with " + std::to_string(gridSize_cm) + "cm grid-size");
	}

	/** no-copy */
	Grid(const Grid& o) = delete;

	/**
	 * reset (clear) the grid
	 * remove all nodes, hashes, ..
	 */
	void reset() {
		nodes.clear();
		hashes.clear();
	}

	/** is the grid empty? */
	bool isEmpty() const {
		return nodes.empty();
	}

	/** no-assign */
	void operator = (const Grid& o) = delete;


	/** allows for-each iteration over all included nodes */
	decltype(nodes.begin()) begin() {return nodes.begin();}

	/** allows for-each iteration over all included nodes */
	decltype(nodes.end()) end() {return nodes.end();}

	/** get the grid's size */
	int getGridSize_cm() const {return gridSize_cm;}

	/**
	 * add the given element to the grid.
	 * returns the index of the element within the internal data-structure
	 * @param elem the element to add
	 */
	int add(const T& elem) {
		assertAligned(elem);							// assert that the to-be-added element is aligned to the grid
		return addUnaligned(elem);
	}

	/** add the given (not necessarly aligned) element to the grid */
	int addUnaligned(const T& elem, const bool check=true) {
		const UID uid = getUID(elem);															// get the UID for this new element
		if (check) {
			Assert::isTrue(hashes.find(uid) == hashes.end(), "node's UID is already taken!");	// avoid potential errors
		}
		const int idx = nodes.size();															// next free index
		nodes.push_back(elem);																	// add it to the grid
		nodes.back()._idx = idx;																// let the node know his own index
		hashes[uid] = idx;																		// add an UID->index lookup
		return idx;																				// done
	}

	/** connect (uni-dir) i1 -> i2 */
	void connectUniDir(const int idx1, const int idx2) {
		connectUniDir(nodes[idx1], nodes[idx2]);
	}

	/** connect (uni-dir) i1 -> i2 */
	void connectUniDir(T& n1, const T& n2) {
		Assert::isFalse(n1.hasNeighbor(n2._idx), "this neighbor is already connected");			// already connected?
		Assert::notEqual(n1.getIdx(), n2.getIdx(), "can not connect node with itself");
		Assert::isFalse(n1.fullyConnected(), "this node has already reached its neighbor-limit");
		n1._neighbors[n1._numNeighbors] = n2._idx;
		++n1._numNeighbors;
	}

	/**
	 * connect (bi-directional) the two provided nodes
	 * @param idx1 index of the first element
	 * @param idx2 index of the second element
	 */
	void connectBiDir(const int idx1, const int idx2) {
		connectBiDir(nodes[idx1], nodes[idx2]);
	}


	/**
	 * connect (bi-directional) the two provided nodes
	 * @param n1 the first node
	 * @param n2 the second node
	 */
	void connectBiDir(T& n1, T& n2) {
		connectUniDir(n1, n2);
		connectUniDir(n2, n1);
	}

	/** get the number of contained nodes */
	int getNumNodes() const {
		return nodes.size();
	}

	/** get the number of neighbors for the given element */
	int getNumNeighbors(const int idx) const {
		return getNumNeighbors(nodes[idx]);
	}

	/** get the number of neighbors for the given element */
	int getNumNeighbors(const T& e) const {
		return e._numNeighbors;
	}

	/** get the n-th neighbor for the given node */
	T& getNeighbor(const int nodeIdx, const int nth) const {
		const T& node = nodes[nodeIdx];
		return getNeighbor(node, nth);
	}

	/** get the n-th neighbor for the given node */
	T& getNeighbor(const T& node, const int nth) const {
		const T& neighbor = nodes[node._neighbors[nth]];
		return (T&) neighbor;
	}

	/** do we have a center-point the given point belongs to? */
	bool hasNodeFor(const GridPoint& p) const {
		const UID uid = getUID(p);
		return (hashes.find(uid) != hashes.end());
	}

	/** get a list of all nodes within the graph */
	const std::vector<T>& getNodes() const {
		return nodes;
	}

	/** get the center-node the given Point belongs to */
	const T& getNodeFor(const GridPoint& p) const {
		//const UID uid = getUID(p);
		auto it = hashes.find(getUID(p));
		Assert::isTrue(it != hashes.end(), "element not found!");
		return nodes[it->second];
	}

	/** get the center-node the given Point belongs to. or nullptr if not present */
	const T* getNodePtrFor(const GridPoint& p) const {
		auto it = hashes.find(getUID(p));
		return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
	}

	/** get the node nearest to the given positon */
	const T& getNearestNode(const GridPoint& p) const {
		auto comp = [p] (const T& a, const T& b) { return a.getDistanceInMeter(p) < b.getDistanceInMeter(p); };
		auto it = std::min_element(nodes.begin(), nodes.end(), comp);
		return nodes[it->getIdx()];
	}

	/** get the node nearest to the given positon, examining only the nodes given by the provided index-array */
	const T& getNearestNode(const GridPoint& p, const std::vector<int>& indices) const {
		auto comp = [&] (const int a, const int b) { return nodes[a].getDistanceInMeter(p) < nodes[b].getDistanceInMeter(p); };
		auto it = std::min_element(indices.begin(), indices.end(), comp);
		return nodes[it->getIdx()];
	}

	/** get the BBox for the given node */
	GridNodeBBox getBBox(const int idx) const {
		return getBBox(nodes[idx]);
	}

	/** get the BBox for the given node */
	GridNodeBBox getBBox(const T& node) const {
		return GridNodeBBox(node, gridSize_cm);
	}

	/** is this node part of a non-plain stair/escalator */
	bool isPlain(const T& n1) const {
		for (const T& n2 : neighbors(n1)) {
			if (n2.z_cm != n1.z_cm) {return false;}
		}
		return true;
	}

	/**
	 * get an UID for the given point.
	 * this works only for aligned points.
	 *
	 */
	UID getUID(const GridPoint& p) const {

		// sanity check (region between -1^19 and +1^19
		const int32_t max = 1 << 19;
		Assert::isBetween((int32_t)p.x_cm, -max, +max, "x out of bounds");
		Assert::isBetween((int32_t)p.y_cm, -max, +max, "y out of bounds");
		Assert::isBetween((int32_t)p.z_cm, -max, +max, "z out of bounds");

		// shift by half of the allowed width of 20 bit to allow negative regions:
		// -> 19 bit positive and 19 bit negative
		const uint64_t center = 1 << 19;

		// build
#if (GRID_MODE == GM_HOBEYCOMB)
		const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2);
		const uint64_t x = center + (int64_t) idxX(p.x_cm-xx);
		const uint64_t y = center + (int64_t) idxY(p.y_cm);
		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
#elif (GRID_MODE == GM_BOX)
		const uint64_t x = center + (int64_t) idxX(p.x_cm);
		const uint64_t y = center + (int64_t) idxY(p.y_cm);
		const uint64_t z = center + (int64_t) idxZ(p.z_cm);
#endif



		return (z << 40) | (y << 20) | (x << 0);

	}

	inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
	inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
	inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes

	inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
	inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
	inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;}	// * 5?? // z is usually much lower and not always aligned -> allow more room for hashes


	/** array access */
	T& operator [] (const int idx) {
		Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");
		return nodes[idx];
	}

	/** const array access */
	const T& operator [] (const int idx) const {
		Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");
		return nodes[idx];
	}

	/** disconnect the two nodes (bidirectional) */
	void disconnectBiDir(const int idx1, const int idx2) {
		disconnectBiDir(nodes[idx1], nodes[idx2]);
	}

	/** disconnect the two nodes (bidirectional) */
	void disconnectBiDir(T& n1, T& n2) {
		disconnectUniDir(n1, n2);
		disconnectUniDir(n2, n1);
	}

	/** remove the connection from n1 to n2 (not the other way round!) */
	void disconnectUniDir(const int idx1, const int idx2) {
		disconnectUniDir(nodes[idx1], nodes[idx2]);
	}

	/** remove the connection from n1 to n2 (not the other way round!) */
	void disconnectUniDir(T& n1, const T& n2) {
		for (int n = 0; n < n1._numNeighbors; ++n) {
			if (n1._neighbors[n] == n2._idx) {
				arrayRemove(n1._neighbors, n, n1._numNeighbors);
				--n1._numNeighbors;
				return;
			}
		}
	}

	/** convert to a GridPoint coordinate (in cm) */
	GridPoint toGridPoint(const Point3 pos_m) const {
		return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) );
	}

	/** remove the given array-index by moving all follwing elements down by one */
	template <typename X> void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) {
		for (int i = idxToRemove+1; i < arrayLen; ++i) {
			arr[i-1] = arr[i];
		}
	}

	/**
	 * mark the given node for deletion
	 * see: cleanup()
	 */
	void remove(const int idx) {
		remove(nodes[idx]);
	}


	/**
	 * mark the given node for deletion
	 * see: cleanup()
	 */
	void remove(T& node) {

		// disconnect from all neighbors
		int cnt = 0;
		while (node._numNeighbors) {

			// by removing one neighbor, all others are shifted to close the gap within the array
			// its therefor ok to always delete array index [0]
			disconnectBiDir(node._idx, node._neighbors[0]);
			if (++cnt > node.MAX_NEIGHBORS) {
				Log::add(name, "WARNING! found unsolveable neighboring! " + node.asString(), true);
				break;
			}
		}

		// remove from hash-list
		hashes.erase(getUID(node));

		// mark for deleteion (see: cleanup())
		node._idx = -1;

	}

	/** remove all nodes marked for deletion */
	void cleanup() {

		debugMemoryUsage();

		Log::add(name, "running grid cleanup", false);
		Log::tick();

		// generate a look-up-table for oldIndex (before deletion) -> newIndex (after deletion)
		std::vector<int> oldToNew; oldToNew.resize(nodes.size());
		int newIdx = 0;
		for (size_t oldIdx = 0; oldIdx < nodes.size(); ++oldIdx) {
			if (nodes[oldIdx].getIdx() != -1) {
				oldToNew[oldIdx] = newIdx;
				++newIdx;
			}
		}

		// adjust all indices from the old to the new mapping
		for (size_t i = 0; i < nodes.size(); ++i) {

			// skip the nodes actually marked for deletion
			if (nodes[i]._idx == -1) {continue;}

			// adjust the node's index
			nodes[i]._idx = oldToNew[nodes[i]._idx];

			// adjust its neighbor's indices
			for (int j = 0; j < nodes[i]._numNeighbors; ++j) {
				nodes[i]._neighbors[j] = oldToNew[nodes[i]._neighbors[j]];
			}

		}

		// MUCH(!!!) faster than deleting nodes from the existing node-vector
		// is to build a new one and swap those two
		std::vector<T> newNodes;
		for (size_t i = 0; i < nodes.size(); ++i) {
			if (nodes[i]._idx != -1) {newNodes.push_back(nodes[i]);}
		}
		std::swap(nodes, newNodes);

		Log::tock();

		rebuildHashes();

		debugMemoryUsage();

	}

	/** rebuild the UID-hash-list */
	void rebuildHashes() {

		Log::add(name, "rebuilding UID hashes", false);
		Log::tick();

		hashes.clear();
		for (size_t idx = 0; idx < nodes.size(); ++idx) {
			hashes[getUID(nodes[idx])] = idx;
		}

		Log::tock();

	}

	/** debug-print the grid's current memory usage */
	void debugMemoryUsage() {
		const uint32_t bytes = nodes.size() * sizeof(T);
		const uint32_t numNodes = nodes.size();
		uint32_t numNeighbors = 0;
		//uint32_t numNeighborsUnused = 0;
		for (T& n : nodes) {
			numNeighbors += n._numNeighbors;
		}
		Log::add(name, "memory: " + std::to_string(bytes/1024.0f/1024.0f) + " MB in " + std::to_string(numNodes) + " nodes");
	}


public:

	/** serialize into the given stream */
	void write(std::ostream& out) {

		// size (in bytes) one node has. this is a sanity check whether the file matches the code!
		const int nodeSize = sizeof(T);
		out.write((const char*) &nodeSize, sizeof(nodeSize));

		// number of nodes
		const int numNodes = nodes.size();
		Assert::isTrue(numNodes > 0, "grid says it contains 0 nodes. there must be some error!");
		out.write((const char*) &numNodes, sizeof(numNodes));

		// serialize
		for (const T& node : nodes) {
			out.write((const char*) &node, sizeof(T));
		}

		// serialize static parameters
		T::staticSerialize(out);

		out.flush();

	}

	/** deserialize from the given stream */
	void read(std::istream& inp) {

		Log::add(name, "loading grid from input-stream");

		// size (in bytes) one node has. this is a sanity check whether the file matches the code!
		int nodeSize;
		inp.read((char*) &nodeSize, sizeof(nodeSize));
		Assert::equal(nodeSize, (int)sizeof(T), "sizeof(node) of the saved grid does not match sizeof(node) for the code!");

		// number of nodes
		int numNodes;
		inp.read((char*) &numNodes, sizeof(numNodes));
		Assert::isTrue(numNodes > 0, "grid-file says it contains 0 nodes. there must be some error!");

		// allocate node-space
		nodes.resize(numNodes);

		// deserialize
		inp.read((char*) nodes.data(), numNodes*sizeof(T));
		Log::add(name, "deserialized " + std::to_string(nodes.size()) + " nodes");

		// deserialize static parameters
		T::staticDeserialize(inp);

		// update
		rebuildHashes();

	}


public:



	NeighborForEach neighbors(const int idx) const {
		return neighbors(nodes[idx]);
	}

	NeighborForEach neighbors(const T& node) const {
		return NeighborForEach(*this, node._idx);
	}

	/** get the grid's bounding-box. EXPENSIVE! */
	BBox3 getBBox() const {
		BBox3 bb;
		for (const T& n : nodes) {
			bb.add( Point3(n.x_cm, n.y_cm, n.z_cm) );
		}
		return bb;
	}

	int kdtree_get_point_count() const {
		return nodes.size();
	}

	template <class BBOX> bool kdtree_get_bbox(BBOX& bb) const { (void) bb; return false; }

	inline float kdtree_get_pt(const size_t idx, const int dim) const {
		const T& p = nodes[idx];
		if (dim == 0) {return p.x_cm;}
		if (dim == 1) {return p.y_cm;}
		if (dim == 2) {return p.z_cm;}
		throw 1;
	}

	inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
		const float d0 = p1[0] - nodes[idx_p2].x_cm;
		const float d1 = p1[1] - nodes[idx_p2].y_cm;
		const float d2 = p1[2] - nodes[idx_p2].z_cm;
		return (d0*d0) + (d1*d1) + (d2*d2);
	}



private:

	/** asssert that the given element is aligned to the grid */
	void assertAligned(const T& elem) {
#if (GRID_MODE == GM_HOBEYCOMB)

#elif (GRID_MODE == GM_BOX)
		if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");}
		if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");}
		//if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");}
#endif
	}

};

#endif // GRID_H