Indoor/grid/factory/v2/GridFactory.h

#ifndef GRIDFACTORY_V2_H
#define GRIDFACTORY_V2_H

#include <string>
#include <unordered_set>
#include <set>

#include "../../../floorplan/v2/Floorplan.h"

#include "Helper.h"
#include "Stairs2.h"
#include "Elevators.h"

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

#include "../../../misc/Debug.h"
#include <functional>


#include "GridFactoryListener.h"


template <typename T> class GridFactory {

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

private:

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

	/** calculation helper */
	Helper<T> helper;

	/** stair builder */
	Stairs<T> stairs;

	/** elevator builder */
	Elevators<T> elevators;


	bool _buildStairs = true;
	bool _removeIsolated = true;

public:

	/** ctor with the grid to fill */
	explicit GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid), elevators(grid) {

	}


	/** whether or not to build stairs */
	void setBuildStairs(const bool build) {this->_buildStairs = build;}

	/** whether or not to remove isolated nodes */
	void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;}


	/** build using the given map */
	void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) {

		Log::add(name, "building grid from IndoorMap", true);

		const int total = map->floors.size()*3 + 1;
		int cur = 0;

		// build all the floors
		if (listener) {listener->onGridBuildUpdateMajor("adding floors");}
		for (Floorplan::Floor* f : map->floors) {
			addFloor(f, listener);
			if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
		}

		// build all stairs
		if (listener) {listener->onGridBuildUpdateMajor("adding stairs");}
		if (_buildStairs) {
			for (Floorplan::Floor* f : map->floors) {
				buildStairs(f, listener);
				if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
			}
		}

		// build all elevators
		if (listener) {listener->onGridBuildUpdateMajor("adding elevators");}
		if (_buildStairs) {
			for (Floorplan::Floor* f : map->floors) {
				buildElevators(f, listener);
				if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
			}
		}

		// remove isolated nodes
		if (_removeIsolated) {
			if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
			removeIsolatedNodes();
			if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
		}


	}

	/** get the 2D-bbox for the given floor's outline */
	BBox2 getFloorOutlineBBox(const Floorplan::FloorOutline& outline) {
		BBox2 bb;
		for (Floorplan::FloorOutlinePolygon* poly : outline) {
			for (Point2 p : poly->poly.points) {
				bb.add(p * 100);		// convert m to cm
			}
		}
		return bb;
	}

	enum class PartOfOutline {
		NO,
		INDOOR,
		OUTDOOR,
	};

	/** get the part of outline the given location belongs to. currently: none, indoor, outdoor */
	static PartOfOutline isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) {

		// assume the point is not part of the outline
		PartOfOutline res = PartOfOutline::NO;

		// process every outline polygon
		for (Floorplan::FloorOutlinePolygon* poly : outline) {
			HelperPoly pol(*poly);
			if (pol.contains(Point2(x_cm, y_cm))) {

				// belongs to a "remove" polygon? -> directly ignore this location!
				if (poly->method == Floorplan::OutlineMethod::REMOVE) {
					return PartOfOutline::NO;
				}

				// belongs to a "add" polygon? -> remember until all polygons were checked
				// [might still belong to a "remove" polygon]
				res = poly->outdoor ? PartOfOutline::OUTDOOR : PartOfOutline::INDOOR;

			}
		}

		// done
		return res;

	}



	/** add the given floor to the grid */
	void addFloor(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {

		Log::add(name, "adding floor '" + floor->name + "'", true);
		if (listener) {listener->onGridBuildUpdateMinor("adding floor " + floor->name);}

		const BBox2 bbox = getFloorOutlineBBox(floor->outline);
		const int x1 = helper.align(bbox.getMin().x);
		const int x2 = helper.align(bbox.getMax().x);
		const int y1 = helper.align(bbox.getMin().y);
		const int y2 = helper.align(bbox.getMax().y);
		const int z_cm = (floor->atHeight*100);

		const int total = (x2-x1) / helper.gridSize();
		int cur = 0;
		int numNodes = 0;

		// build grid-points for floor-outline
		for(int x_cm = x1; x_cm < x2; x_cm += helper.gridSize()) {
			for (int y_cm = y1; y_cm < y2; y_cm += helper.gridSize()) {

				// does the outline-polygon contain this position?
				const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline);
				if (part == PartOfOutline::NO) {continue;}

				// check intersection with the floorplan
				GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());

				// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
				bbox.grow(0.42345);
				if (intersects(bbox, floor)) {continue;}

				// add to the grid [once]
				T t(x_cm, y_cm, z_cm);
				if (grid.hasNodeFor(t)) {continue;}
				updateType(t, part, bbox, floor);
				grid.add(t);


				// debug
				++numNodes;

			}

			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}

		}

		Log::add(name, "added " + std::to_string(numNodes) + " nodes");

		// connect the g
		connectAdjacent(floor, z_cm);

	}

	void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {

		const int total = floor->stairs.size();
		int cur = 0;

		// process each stair within the floor
		for (const Floorplan::Stair* stair : floor->stairs) {
			if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));}
			stairs.build(floor, stair);
			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
		}

		// cleanup
		stairs.finalize();

	}

	void buildElevators(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {

		const int total = floor->elevators.size();
		int cur = 0;

		// process each elevator within the floor
		for (const Floorplan::Elevator* elevator : floor->elevators) {
			if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " elevator " + std::to_string(cur+1));}
			elevators.build(floor, elevator);
			if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
		}

	}




	/** 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 Floorplan::Floor* floor, const int z_cm) {

		Log::add(name, "connecting all adjacent nodes within floor '" + floor->name + "' (atHeight: " + std::to_string(z_cm) + "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(floor, n1);

		}

		Log::tock();

	}

	/**
	 * connect the given node with its neighbors.
	 * even though a node has a neighbor, it might still be blocked by an obstacle:
	 * e.g. a 45° wall directly between nodes. a neighbor exists, but is unreachable due to the wall.
	 * we thus perform an additional intersection check with all obstacles within the floor the node n1 belongs to
	 */
	void connectAdjacent(const Floorplan::Floor* floor, T& n1) {

		const int gridSize_cm = grid.getGridSize_cm();

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

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

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

				// does the grid contain the potential neighbor?
				const T* n2 = grid.getNodePtrFor(p);
				if (n2 != nullptr) {
					if (isBlocked(floor, n1, *n2)) {continue;}		// is there a (e.g. small) obstacle between the two?
					grid.connectUniDir(n1, *n2);			// UNI-dir connection as EACH node is processed!
				}

			}
		}

	}

	/** 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 removeIsolatedNodes() {

//		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();

//	}

	void removeIsolatedNodes() {

		// try to start at the first stair
		for (T& n : grid) {
			if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
		}

		// no stair found? try to start at the first node
		removeIsolatedNodes(grid[0]);

	}

	/** remove all nodes not connected to n1 */
	void removeIsolatedNodes(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());
				}
			}

		}

	}

private:



	/** does the bbox intersect with any of the floor's walls? */
	static inline bool intersects(const GridNodeBBox& bbox, const Floorplan::Floor* floor) {

		// process each obstacle
		// (obstacles use meter, while the bbox is in centimeter!
		for (Floorplan::FloorObstacle* fo : floor->obstacles) {

			// depends on the type of obstacle
			if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
				const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
				const Line2 l2(line->from*100, line->to*100);
				if (bbox.intersects(l2)) {return true;}

			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
				const Point2 center = bbox.getCenter();
				const float dist = center.getDistance(circle->center*100);
				if (dist < circle->radius*100) {return true;}

			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
				// DOORS ARE NOT AN OBSTACLE

			} else {
				throw Exception("TODO: not yet implemented obstacle type");

			}

		}

		return false;

	}

	/**
	 * normally, the algorithm will not try to connect two nodes that are neighbors but have an obstacle between them.
	 * however, especially for 45° obstacles it might happen that a neighbor exists but is blocked by an obstacle.
	 * we thus need this additional check to ensure everything is fine... even though it needs performance...
	 */
	static inline bool isBlocked(const Floorplan::Floor* floor, const GridPoint& n1, const GridPoint& n2) {

		// (obstacles use meter, while the nodes are in centimeter!
		const Point2 p1_m = n1.inMeter().xy();
		const Point2 p2_m = n2.inMeter().xy();
		const Line2 lineNodes(p1_m, p2_m);

		// process each obstacle
		for (Floorplan::FloorObstacle* fo : floor->obstacles) {

			// depends on the type of obstacle
			if (dynamic_cast<Floorplan::FloorObstacleLine*>(fo)) {
				const Floorplan::FloorObstacleLine* line = (Floorplan::FloorObstacleLine*) fo;
				const Line2 lineObstacle(line->from, line->to);
				if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;}

			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
				throw Exception("should not happen");

			} else if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
				// DOORS ARE NOT AN OBSTACLE

			} else {
				throw Exception("TODO: not yet implemented obstacle type");

			}

		}
		return false;

	}

	/** adjust the given gridNode's type if needed [e.g. from "floor" to "door"] */
	static inline void updateType(T& t, const PartOfOutline part, const GridNodeBBox& bbox, const Floorplan::Floor* floor) {

		// first, assume the type of the outline polygon
		switch (part) {
			case PartOfOutline::OUTDOOR:	t.setType(GridNode::TYPE_OUTDOOR); break;
			case PartOfOutline::INDOOR:		t.setType(GridNode::TYPE_FLOOR); break;
			default:						throw Exception("should not happen");
		}

		// hereafter, process each obstacle and mark doors
		for (Floorplan::FloorObstacle* fo : floor->obstacles) {

			if (dynamic_cast<Floorplan::FloorObstacleDoor*>(fo)) {
				const Floorplan::FloorObstacleDoor* door = (Floorplan::FloorObstacleDoor*) fo;
				const Line2 l2(door->from*100, door->to*100);
				if (bbox.intersects(l2)) {
					t.setType(GridNode::TYPE_DOOR);
					return;	// done
				}
			}

		}

	}


};

#endif // GRIDFACTORY_V2_H