#ifndef GRIDFACTORY_H
#define GRIDFACTORY_H

#include <string>
#include <unordered_set>

#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.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
				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 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();

		const int gridSize_cm = grid.getGridSize_cm();

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

			// square around each point
			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!
					}

				}
			}

		}

		Log::tock();

	}


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

		for (const Stair& s : stairs) {

			for (int i = 0; i < grid.getNumNodes(); ++i) {

				// potential starting-point for the stair
				T& n = (T&) grid[i];

				// real starting point for the stair?
				if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {

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

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

						buildStair(n, n2);

					}

				}
			}

		}

	}

	/** build a stair (z-transition) from n1 to n2 */
	void buildStair(T& n1, T& n2) {

		//TODO: ensure n1 is below n2
		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();
		int idx2 = -1;
		const int idx3 = n2.getIdx();

		const int gridSize_cm = grid.getGridSize_cm();

		// move upards in gridSize steps
		for (int z = gridSize_cm; z < zDiff; 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;

			// 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
		if (idx2 != -1) {
			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();

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

	}

private:

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

	}

private:

	/** does the bbox intersect with any of the floor's walls? */
	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