#ifndef GRID_H
#define GRID_H

#include <vector>
#include <unordered_map>

#include "../Exception.h"
#include "GridPoint.h"
#include "GridNode.h"
#include <iostream>

#include <KLib/Assertions.h>
#include "../geo/BBox3.h"

/**
 * grid of the given grid-size, storing some value which
 * extends GridPoint
 */
template <int gridSize_cm, typename T> class Grid {

typedef uint64_t UID;

private:

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

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

public:

	/** ctor */
	Grid() {
		static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!");
		static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!");
	}

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

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


	/**
	 * 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 int idx = nodes.size();					// next free index
		const UID uid = getUID(elem);					// get the UID for this new element
		nodes.push_back(elem);							// add it to the grid
		nodes.back()._idx = idx;
		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) {
		n1._neighbors[n1._numNeighbors] = n2._idx;
		++n1._numNeighbors;
		if (n1._numNeighbors > 12) {
			int i = 0;
		}
		_assertBetween(n1._numNeighbors, 0, 12, "number of neighbors out of bounds!");
	}

	/**
	 * 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 the center-node the given Point belongs to */
	const T& getNodeFor(const GridPoint& p) {
		const UID uid = getUID(p);
		if (hashes.find(uid) == hashes.end()) {throw Exception("element not found!");}
		return nodes[hashes[uid]];
	}

	/** 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);
	}

	/**
	 * get an UID for the given point.
	 * this works only for aligned points.
	 *
	 */
	UID getUID(const GridPoint& p) const {
		const uint64_t x = std::round(p.x_cm / gridSize_cm);
		const uint64_t y = std::round(p.y_cm / gridSize_cm);
		const uint64_t z = std::round(p.z_cm / gridSize_cm);
		return (z << 40) | (y << 20) | (x << 0);
	}

	/** array access */
	T& operator [] (const int idx) {
		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(T& n1, 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;
			}
		}
	}

	/** 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]);
	}

	void remove(T& node) {

		// disconnect from all neighbors
		while (node._numNeighbors) {
			disconnectBiDir(node._idx, node._neighbors[0]);
		}

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

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

	}

	/**
	 * remove all nodes, marked for deletion.
	 * BEWARE: this will invalidate all indices used externally!
	 */
	void cleanup() {

		for (size_t i = 0; i < nodes.size(); ++i) {
			if (nodes[i]._idx == -1) {
				nodes.erase(nodes.begin()+i);
				moveDown(i);
				--i;
			}
		}

	}

	void moveDown(const int idx) {
		for (size_t i = 0; i < nodes.size(); ++i) {
			if (nodes[i]._idx >= idx) {--nodes[i]._idx;}
			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
			}
		}
	}

	class NeighborIter : std::iterator<std::input_iterator_tag, int> {
	private:
		Grid<gridSize_cm, T>& grid;
		int nodeIdx;
		int nIdx;
	public:
		NeighborIter(Grid<gridSize_cm, T>& grid, const int nodeIdx, const int nIdx) : grid(grid), nodeIdx(nodeIdx), nIdx(nIdx) {;}
		NeighborIter& operator++() {++nIdx; return *this;}
		NeighborIter operator++(int) {NeighborIter tmp(*this); operator++(); return tmp;}
		bool operator==(const NeighborIter& rhs) {return nodeIdx == rhs.nodeIdx && nIdx == rhs.nIdx;}
		bool operator!=(const NeighborIter& rhs) {return nodeIdx != rhs.nodeIdx || nIdx != rhs.nIdx;}
		T& operator*() {return (T&) grid.nodes[nodeIdx]._neighbors[nIdx];}
	};

	class NeighborForEach {
	private:
		Grid<gridSize_cm, T>& grid;
		int nodeIdx;
	public:
		NeighborForEach(Grid<gridSize_cm, T>& grid, const int nodeIdx) : grid(grid), nodeIdx(nodeIdx) {;}
		NeighborIter begin() {return NeighborIter(grid, nodeIdx, 0);}
		NeighborIter end() {return NeighborIter(grid, nodeIdx, grid[nodeIdx]._numNeighbors);}
	};

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

	NeighborForEach neighbors(const T& node) {
		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 { 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 (((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 // GRID_H