#ifndef GRIDFACTORY_H
#define GRIDFACTORY_H

#include <string>
#include "../../floorplan/Floor.h"
#include "../../floorplan/Stairs.h"

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

template <int gridSize_cm, typename T> class GridFactory {

private:

	Grid<gridSize_cm, T>& grid;


public:

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

	/** add the given floor at the provided height (in cm) */
	void addFloor(const Floor& floor, const float z_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));

			}
		}

		connectAdjacent(z_cm);

	}

	void connectAdjacent(const float z_cm) {

		// connect adjacent grid-points
		for (int idx = 0; idx < grid.getNumNodes(); ++idx) {

			T& n1 = (T&) grid[idx];
			if (n1.z_cm != z_cm) {continue;}	// ugly... different floor -> skip

			// 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
					int ox = n1.x_cm + x;
					int oy = n1.y_cm + y;
					GridPoint p(ox, oy, n1.z_cm);

					// does the grid contain the potential neighbor?
					if (grid.hasNodeFor(p)) {
						T& n2 = (T&) grid.getNodeFor(p);
						grid.connectUniDir(n1, n2);
					}

				}
			}

		}

	}


	void addStairs(const Stairs& stairs, const float z1_cm, const float 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);

					}
					int i = 0;
				}
			}

		}

	}

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

		// 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<gridSize_cm, T>& gIn, const float z_cm) {

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

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

		// get largest connected region
		std::set<int> set;
		do {
			const int idxStart = rand() % grid.getNumNodes();
			set.clear();
			getConnected(idxStart, set);
		} while (set.size() < 0.5 * grid.getNumNodes());


		// remove all other
		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::set<int>& set) {

		T& n1 = (T&) grid[idx];
		set.insert(n1.getIdx());

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