Indoor/grid/factory/GridFactory.h

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

#include <string>
#include <unordered_set>

#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.h"
#include "../../floorplan/PlatformStair.h"

#include "../../geo/Units.h"
#include "../GridNodeBBox.h"
#include "../Grid.h"

#include "../../misc/Debug.h"

template <typename T> class GridFactory {

	/** logging name */
	static constexpr const char* name = "GridFac";

private:

	/** the grid to build into */
	Grid<T>& grid;


public:

	/** ctor with the grid to fill */
	GridFactory(Grid<T>& grid) : grid(grid) {;}

	/** add the given floor at the provided height (in cm) */
	void addFloor(const Floor& floor, const float z_cm) {

		Log::add(name, "adding floor at height " + std::to_string(z_cm), false);
		Log::tick();

		const int gridSize_cm = grid.getGridSize_cm();

		// build grid-points
		for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
			for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {

				// check intersection with the floorplan
				const GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
				if (intersects(bbox, floor)) {continue;}

				// add to the grid
				grid.add(T(x_cm, y_cm, z_cm));

			}
		}

		Log::tock();

		connectAdjacent(z_cm);

	}

	/** connect all neighboring nodes part of the given index-vector */
	void connectAdjacent(const std::vector<int>& indices) {

		for (const int idx : indices) {

			// connect the node with its neighbors
			connectAdjacent(grid[idx]);

		}

	}

	/** connect all neighboring nodes located on the given height-plane */
	void connectAdjacent(const float z_cm) {

		Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
		Log::tick();

		// connect adjacent grid-points
		for (T& n1 : grid) {

			// not the floor we are looking for? -> skip (ugly.. slow(er))
			if (n1.z_cm != z_cm) {continue;}

			// connect the node with its neighbors
			connectAdjacent(n1);

		}

		Log::tock();

	}

	/** connect the given node with its neighbors */
	void connectAdjacent(T& n1) {

		const int gridSize_cm = grid.getGridSize_cm();

		// square around the node
		for (int x =  -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
			for (int y =  -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {

				// skip the center (node itself)
				if ((x == y) && (x == 0)) {continue;}

				// position of the potential neighbor
				const int ox = n1.x_cm + x;
				const int oy = n1.y_cm + y;
				const GridPoint p(ox, oy, n1.z_cm);

				// does the grid contain the potential neighbor?
				const T* n2 = grid.getNodePtrFor(p);
				if (n2 != nullptr) {
					grid.connectUniDir(n1, *n2);			// UNI-dir connection as EACH node is processed!
				}

			}
		}

	}

	float align(const float val) {
		const float gridSize_cm = grid.getGridSize_cm();
		return std::round(val/gridSize_cm) * gridSize_cm;
	}

	/** shrink the given bbox to be grid-aligned */
	BBox2 shrinkAlign(const BBox2& bb) {
		const float gridSize_cm = grid.getGridSize_cm();
		Point2 p1 = bb.getMin();
		Point2 p2 = bb.getMax();
		p1.x = std::ceil(p1.x/gridSize_cm)*gridSize_cm;
		p1.y = std::ceil(p1.y/gridSize_cm)*gridSize_cm;
		p2.x = std::floor(p2.x/gridSize_cm)*gridSize_cm;
		p2.y = std::floor(p2.y/gridSize_cm)*gridSize_cm;
		BBox2 res; res.add(p1); res.add(p2); return res;
	}

	/** add a new platform-stair between the two given floors */
	void buildPlatformStair(const PlatformStair& s, const float z1_cm, const float z2_cm) {

		const float zCenter_cm = align((z2_cm + z1_cm) / 2);
		std::vector<int> indices;

		// add the platform in the middle
		BBox2 bb = shrinkAlign(s.platform);
		const int gridSize_cm = grid.getGridSize_cm();
		for (int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
			for (int y_cm = bb.getMin().y; y_cm <= bb.getMax().y; y_cm += gridSize_cm) {
				int idx = grid.add(T(x_cm, y_cm, zCenter_cm));
				indices.push_back(idx);
			}
		}

		// connect the plattform in the middle
		connectAdjacent(indices);

		// TODO: interconnect (x-y) the stair lines???
		buildStair(s.s1, z1_cm, zCenter_cm);
		buildStair(s.s2, z2_cm, zCenter_cm);

	}

	void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {

		Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
		Log::tick();

		for (const Stair& s : stairs) { buildStair(s, z1_cm, z2_cm); }

		Log::tock();

	}

	void buildStair(const Stair& s, const float z1_cm, const float z2_cm) {

		// potential starting-point for the stair
		for (T& n : grid) {

			// node lies on the stair's starting edge?
			if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {

				// construct end-point by using the stair's direction
				const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
				GridPoint gp(end.x, end.y, end.z);

				// does such and end-point exist within the grid? -> construct stair-line
				if (grid.hasNodeFor(gp)) {
					T& n2 = (T&) grid.getNodeFor(gp);
					buildStairLine(grid, n, n2);
				}

			}
		}

	}

	/**
	 * this method ensures that the start- and end-node of a stair are "terminated":
	 * they have less than 8 neighbors. it must not be possible to walk on otherwise
	 * than using the stair.
	 *
	 * NOTE: only works for axis aligned stairs!!!!
	 * NOTE: needs grid.cleanup() during some later timestep!!
	 *
	 */
	void removeStairBlocked(Grid<T>& grid, T& _n1, T& _n2) {

		Point3 pStart(_n1.x_cm, _n1.y_cm, _n1.z_cm);
		Point3 pEnd(_n2.x_cm, _n2.y_cm, _n2.z_cm);

		// cleanup the region around the starting node
		BBox3 bboxStart;
		bboxStart.add( pStart );						// from the start
		bboxStart.add( pStart + (pEnd-pStart) * 0.3 );	// 30% to the end

		// cleanup the region around the destionation node
		BBox3 bboxEnd;
		bboxEnd.add( pEnd );							// from the end
		bboxEnd.add( pEnd + (pStart-pEnd) * 0.3 );		// 30% to the start

		for (T& n : grid) {
			if (n == _n1) {continue;}	// keep the start itself
			if (n == _n2) {continue;}	// keep the end itself
			Point3 pp(n.x_cm, n.y_cm, n.z_cm);
			if		(bboxStart.contains(pp))	{grid.remove(n);} // cleanup starting region
			else if	(bboxEnd.contains(pp))		{grid.remove(n);} // cleanup ending region
		}


	}

	/** build a stair (z-transition) from n1 to n2 */
	void buildStairLine(Grid<T>& grid, T& _n1, T& _n2) {

		// remove unneccesary nodes in stair-environments
		removeStairBlocked(grid, _n1, _n2);

		// does not work, deletion takes place at a later time!
		// sanity check: both, the starting and the end node should have less than 8 neighbors, otherwise they are not "leading" into the stair
		//if (_n1.getNumNeighbors() >= 8) {throw Exception("invalid number of neighbors for a stair-start-node");}
		//if (_n2.getNumNeighbors() >= 8) {throw Exception("invalid number of neighbors for a stair-end-node");}

		// half the grid size = small steps
		const int gridSize_cm = grid.getGridSize_cm() / std::sqrt(2);

		// local copies, needed for std::swap to work
		T n1 = _n1; T n2 = _n2;

		// ensure we work from lower to upper levels
		if (n2.z_cm < n1.z_cm) { std::swap(n1, n2); }

		// dimensional difference between stairt start and end
		const float zDiff = n2.z_cm - n1.z_cm;
		const float xDiff = n2.x_cm - n1.x_cm;
		const float yDiff = n2.y_cm - n1.y_cm;

		int idx1 = n1.getIdx();				// starting node
		int idx2 = -1;						// next node
		const int idx3 = n2.getIdx();		// final node

		// move upards
		for (int _z = gridSize_cm; _z <= zDiff - gridSize_cm; _z+= gridSize_cm) {

			// calculate the percentage of reached upwards-distance
			const float percent = _z/zDiff;

			// adjust (x,y) accordingly (interpolate)
			int x = n1.x_cm + xDiff * percent;
			int y = n1.y_cm + yDiff * percent;
			int z = n1.z_cm + _z;

			// snap (x,y) to the grid???
			//x = std::round(x / gridSize_cm) * gridSize_cm;
			//y = std::round(y / gridSize_cm) * gridSize_cm;

			// create a new node add it to the grid, and connect it with the previous one
			idx2 = grid.addUnaligned(T(x,y,z));
			grid.connectBiDir(idx1, idx2);
			idx1 = idx2;

		}

		// add the last segment
		Assert::isTrue(idx2 != -1, "strange stair issue?!");
		grid.connectBiDir(idx2, idx3);

	}

	/** add the inverted version of the given z-layer */
	void addInverted(const Grid<T>& gIn, const float z_cm) {

		// get the original grid's bbox
		BBox3 bb = gIn.getBBox();

		// ensure we have an outer boundary
		bb.grow(gIn.getGridSize_cm() * 2);

		const int gridSize_cm = grid.getGridSize_cm();

		// build new grid-points
		for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
			for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {

				// does the input-grid contain such a point?
				GridPoint gp(x_cm, y_cm, z_cm);
				if (gIn.hasNodeFor(gp)) {continue;}

				// add to the grid
				grid.add(T(x_cm, y_cm, z_cm));

			}
		}

	}

	// TODO: how to determine the starting index?!

	// IDEAS: find all segments:
	// start at a random point, add all connected points to the set
	// start at a NEW random point ( not part of the already processed points), add connected points to a new set
	// repeat until all points processed
	// how to handle multiple floor layers?!?!
	// run after all floors AND staircases were added??
	// OR: random start, check segment size, < 50% of all nodes? start again

	void removeIsolated() {

		Log::add(name, "searching for isolated nodes");

		// get largest connected region
		std::unordered_set<int> set;
		do {
			const int idxStart = rand() % grid.getNumNodes();
			set.clear();
			Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
			getConnected(grid[idxStart], set);
			Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");

		} while (set.size() < 0.5 * grid.getNumNodes());

		// remove all other
		Log::add(name, "removing the isolated nodes");
		for (int i = 0; i < grid.getNumNodes(); ++i) {
			if (set.find(i) == set.end()) {grid.remove(i);}
		}

		// clean the grid
		grid.cleanup();

	}

	/** remove all nodes not connected to n1 */
	void removeIsolated(T& n1) {

		// get the connected region around n1
		Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
		Log::tick();
		std::unordered_set<int> set;
		getConnected(n1, set);
		Log::tock();

		// remove all other
		Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
		Log::tick();
		for (T& n2 : grid) {
			if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
		}
		Log::tock();

		// clean the grid (physically delete the removed nodes)
		grid.cleanup();

	}

private:

	/** recursively get all connected nodes and add them to the set */
	void getConnected(T& n1, std::unordered_set<int>& visited) {

		std::unordered_set<int> toVisit;
		toVisit.insert(n1.getIdx());

		// run while there are new nodes to visit
		while(!toVisit.empty()) {

			// get the next node
			int nextIdx = *toVisit.begin();
			toVisit.erase(nextIdx);
			visited.insert(nextIdx);
			T& next = grid[nextIdx];

			// get all his (unprocessed) neighbors and add them to the region
			for (const T& n2 : grid.neighbors(next)) {
				if (visited.find(n2.getIdx()) == visited.end()) {
					toVisit.insert(n2.getIdx());
				}
			}

		}

	}

//	/** recursively get all connected nodes and add them to the set */
//	void getConnected(const int idx, std::unordered_set<int>& set) {

//		// get the node behind idx
//		const T& n1 = (T&) grid[idx];

//		// add him to the current region
//		set.insert(n1.getIdx());

//		// get all his (unprocessed) neighbors and add them to the region
//		for (const T& n2 : grid.neighbors(n1)) {
//			if (set.find(n2.getIdx()) == set.end()) {
//				getConnected(n2.getIdx(), set);
//			}
//		}

//	}

//	/** recursively get all connected nodes and add them to the set */
//	void getConnected(const T& n1, std::unordered_set<int>& set) {

//		// add him to the current region
//		set.insert(n1.getIdx());

//		// get all his (unprocessed) neighbors and add them to the region
//		for (const T& n2 : grid.neighbors(n1)) {
//			if (set.find(n2.getIdx()) == set.end()) {
//				getConnected(n2, set);
//			}
//		}

//	}

private:

	/** does the bbox intersect with any of the floor's walls? */
	static inline bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
		for (const Line2& l : floor.getObstacles()) {
			if (bbox.intersects(l)) {return true;}
		}
		return false;
	}


};

#endif // GRIDFACTORY_H