Indoor/grid/Grid.h

#ifndef GRID_H
#define GRID_H

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

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

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

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

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

	/** 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 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;
		_assertBetween(n1._numNeighbors, 0, 10, "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 center-node the given Point belongs to. or nullptr if not present */
	const T* getNodePtrFor(const GridPoint& p) {
		auto it = hashes.find(getUID(p));
		return (it == hashes.end()) ? (nullptr) : (&nodes[it->second]);
	}

	/** 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 / (float)gridSize_cm);
		const uint64_t y = std::round(p.y_cm / (float)gridSize_cm);
		const uint64_t z = std::round(p.z_cm / (float)gridSize_cm);
		return (z << 40) | (y << 20) | (x << 0);
	}

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

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

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

		// check every single node
		for (size_t i = 0; i < nodes.size(); ++i) {

			// is this node marked as "deleted"? (idx == -1)
			if (nodes[i]._idx == -1) {

				// remove this node
				deleteNode(i);
				--i;

			}
		}

		// rebuild hashes
		Log::add(name, "rebuilding UID hashes");
		hashes.clear();
		for (size_t idx = 0; idx < nodes.size(); ++idx) {
			hashes[getUID(nodes[idx])] = idx;
		}

	}

private:

	/** hard-delete the given node */
	void deleteNode(const int idx) {

		_assertBetween(idx, 0, nodes.size()-1, "index out of bounds");

		// COMPLEX AND SLOW AS HELL.. BUT UGLY TO REWIRTE TO BE CORRECT

		// remove him from the node list (reclaim its memory and its index)
		nodes.erase(nodes.begin()+idx);

		// decrement the index for all of the following nodes and adjust neighbor references
		for (size_t i = 0; i < nodes.size(); ++i) {

			// decrement the higher indices (reclaim the free one)
			if (nodes[i]._idx >= idx) { --nodes[i]._idx;}

			// adjust the neighbor references (decrement by one)
			for (int n = 0; n < nodes[i]._numNeighbors; ++n) {
				if (nodes[i]._neighbors[n] >= idx) {--nodes[i]._neighbors[n];}
			}

		}
	}

public:



	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 { (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 (((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