#ifndef GRID_H
#define GRID_H

#include <vector>
#include <unordered_map>

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

/**
 * 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
		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 (bi-directional) the two provided nodes
	 * @param idx1 index of the first element
	 * @param idx2 index of the second element
	 */
	void connect(const int idx1, const int idx2) {
		T& n1 = nodes[idx1];				// get the first element
		T& n2 = nodes[idx2];				// get the second element
		connect(n1, n2);
	}

	/**
	 * connect (bi-directional) the two provided nodes
	 * @param n1 the first node
	 * @param n2 the second node
	 */
	void connect(T& n1, T& n2) {
		n1._neighbors[n1._numNeighbors] = n2._idx;				// add them both as neighbors
		n2._neighbors[n2._numNeighbors] = n1._idx;
		++n1._numNeighbors;										// increment the neighbor-counter
		++n2._numNeighbors;
	}

	/** 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 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 */
	const T& operator [] (const int idx) const {
		return nodes[idx];
	}

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