Indoor/grid/factory/v2/GridFactory.h

#ifndef GRIDFACTORY_V2_H
#define GRIDFACTORY_V2_H

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

#include "../../../math/Math.h"

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

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

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

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

#include "../../../wifi/estimate/ray3/OBJPool.h"
#include "../../../geo/ConvexHull2.h"

#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;


	std::vector<GridPoint> withinRemovePolygon;

	bool _buildStairs = true;
	bool _removeIsolated = true;
	bool _addTightToObstacle = false;

public:

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

	}

	void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;}

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


	/** does the given grid-node have a stair-neighbor? */
	bool hasStairNeighbor(const T& node) const {
		for (const T& n : grid.neighbors(node)) {
			if (n.getType() == GridNode::TYPE_STAIR) {return true;}
		}
		return false;
	}

	/** 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(map, 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 nodes within remove-OutlinePolygons
		// this must happen AFTER stairs have been added, otherwise stairs might not be connectable due to removed/missing nodes.
		// also, within this loop we prevent the deltion of nodes that are a direct neighbor of a stair!
		// [otherwise the same thing would happen again!]
		if (true) {

			for (const GridPoint& gp : withinRemovePolygon) {
				T* n = (T*) grid.getNodePtrFor(gp);

				// delete if node is present and is no direct neighbor of a stair
				if (n && !hasStairNeighbor(*n)) {
					grid.remove(*n);
				}

			}

			// remove all nodes that were just marked for removal
			grid.cleanup();

		}

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

	struct PartOfOutline {
		bool contained = false;			// contained within at-least one polygon?
		bool markedForRemove = false;	// part of a "to-be-removed" polygon?
		bool outdoor = false;			// otherwise: indoor
	};

	/** 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;
		res.contained = false;
		res.markedForRemove = false;

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

				// mark as "contained"
				res.contained = true;

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

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

			}
		}

		// 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 = std::round(floor->atHeight*100);

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

		// all 3D objects within the floor
		std::vector<HelperPoly> objObstacles;
		for (const Floorplan::FloorObstacle* fo : floor->obstacles) {

			// process all object-obstalces
			const Floorplan::FloorObstacleObject* foo = dynamic_cast<const Floorplan::FloorObstacleObject*>(fo);
			if (foo) {

				// get the obstacle
				const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos);

				// construct its 2D convex hull (in centimter)
				HelperPoly poly;
				for (const Point2 p : ConvexHull2::get(obs.getPoints2D())) {poly.add(p*100);}
				objObstacles.push_back(poly);

			}
		}

		// does any of the obj-obstalces contain the given point?
		auto isPartOfObject = [&objObstacles, this] (const GridNodeBBox& bb) {
			for (HelperPoly poly : objObstacles) {
				//if (!_addTightToObstacle) {
					if (poly.contains(bb.getCorner1())) {return true;}
					if (poly.contains(bb.getCorner2())) {return true;}
					if (poly.contains(bb.getCorner3())) {return true;}
					if (poly.contains(bb.getCorner4())) {return true;}
				//} else {
				//	//poly.shrink(1);
				//	if (poly.contains(bb.getCenter())) {return true;}
				//}
			}
			return false;
		};

		// 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.contained) {continue;}

				// bbox to 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.1337);
				if (!_addTightToObstacle) {
					if (intersects(bbox, floor)) {continue;}
				}

				// intersection with objects?
				if (isPartOfObject(bbox)) {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);

				// node part of remove-region?
				// if so, remove >>AFTER<< stairs have been added
				if (part.markedForRemove) {
					withinRemovePolygon.push_back(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::IndoorMap* map, 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(map, 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?

					// NOTE
					// if you have two floors at the >same< height, and their outlines overlap, this one is needed!
					if (!n1.hasNeighbor(n2->getIdx())) {
						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() {

		//std::cout << "todo: remove" << std::endl;
		//return;

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

		//const int numToRemove = grid.getNumNodes() - set.size();
		//int numRemoved = 0;

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

				// sanity check
				// wouldn't make sense that a stair-node is removed..
				// maybe something went wrong elsewhere???
				Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!");
				Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!");
				//Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!");

				// proceed ;)
				grid.remove(n2);

				//++numRemoved;
				//std::cout << numRemoved << ":" << numToRemove << std::endl;

			}
		}
		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:

	/** as line-obstacles have a thickness, we need 4 lines for the intersection test! */
	static std::vector<Line2> getThickLines(const Floorplan::FloorObstacleLine* line) {
		//const Line2 base(line->from*100, line->to*100);
		const float thickness_m = line->thickness_m;
		const Point2 dir = (line->to - line->from);				// obstacle's direction
		const Point2 perp = dir.perpendicular().normalized();	// perpendicular direction (90 degree)
		const Point2 p1 = line->from + perp * thickness_m/2;		// start-up
		const Point2 p2 = line->from - perp * thickness_m/2;		// start-down
		const Point2 p3 = line->to + perp * thickness_m/2;		// end-up
		const Point2 p4 = line->to - perp * thickness_m/2;		// end-down
		return {
			Line2(p1, p2),
			Line2(p3, p4),
			Line2(p2, p4),
			Line2(p1, p3),
		};
	}

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

				// old method (does not support thickness)
				//const Line2 l2(line->from*100, line->to*100);
				//if (bbox.intersects(l2)) {return true;}
				const std::vector<Line2> lines = getThickLines(line);
				for (const Line2& l : lines) {
					if (bbox.intersects(l*100)) {return true;}
				}


			} else if (dynamic_cast<Floorplan::FloorObstacleCircle*>(fo)) {
				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
				const float dist = std::min(
					bbox.getCorner1().getDistance(circle->center*100),
					bbox.getCorner2().getDistance(circle->center*100),
					bbox.getCorner3().getDistance(circle->center*100),
					bbox.getCorner4().getDistance(circle->center*100)
				);
				const float threshold = circle->radius * 100;
				if (dist < threshold) {return true;}

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

			} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
				// ADDED EARLIER

			} 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)) {
				const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo;
				Circle2 circ(circle->center, circle->radius);
				if (circ.intersects(lineNodes)) {return true;}
				//throw Exception("should not happen");

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

			} else if (dynamic_cast<Floorplan::FloorObstacleObject*>(fo)) {
				// removed earlier
				//std::cout << "GridFactory: TODO: Floorplan::FloorObstacleObject" << std::endl;

			} 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
		if (part.outdoor) {
			t.setType(GridNode::TYPE_OUTDOOR);
		} else {
			//t.setType(GridNode::TYPE_FLOOR);
		}

		// 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