#ifndef STAIRS_H
#define STAIRS_H

#include <vector>
#include <set>

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

#include <fstream>
#include <unordered_set>

/**
 * this one should be both, simpler and more robus than v1.
 * - prevents duplicate (x,y) nodes
 * - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm)
 * - only connects the corener nodes between adjacent quads (seems better for most stair-situations)
 *
 */
template <typename T> class Stairs {

private:

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

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

	// keep a list of all vertices below stairwells and remove them hereafter
	std::vector<T*> toDelete;

	bool tryImproveStairConnections = true;

private:

	/** helper struct */
	struct StairNode {

		int x_cm;
		int y_cm;
		int belongsToQuadIdx;

		float z_cm;			// interpolated
		int gridIdx = -1;

		StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;}


	};


public:

	/** ctor */
	Stairs(Grid<T>& grid) : grid(grid), helper(grid) {

	}

	~Stairs() {
		finalize();
	}





	void build(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, const Floorplan::Stair* stair) {

		// starting-height of all available floors (in cm)
		// needed to ensure that stairs start and end at floors
		std::vector<int> floorsAtHeight_cm;
		for (const Floorplan::Floor* f : map->floors) {
			const int floorAtHeight_cm = std::round(f->atHeight*100);
			floorsAtHeight_cm.push_back(floorAtHeight_cm);
		}

		const int gs_cm = grid.getGridSize_cm();

		// get all of the parts for the stair
		const std::vector<Floorplan::StairPart> parts = stair->getParts();

		// convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps!
		const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor, 1.05);

		std::vector<StairNode> stairNodes;

		// process each quad to get a list of all stair-nodes to add
		for (int i = 0; i < (int) quads.size(); ++i) {

			// create a 2D polygon (ignore z) for the quad
			const Floorplan::Quad3& quad = quads[i];
			HelperPoly poly(quad);

			// get a callback for each node that belongs to the quad
			poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) {

				const StairNode sn(x_cm, y_cm, i);

				// IMPORTANT
				// skip nodes that belong to more than one quad -> skip duplicates -> much more stable!
				// NOTE: currently this will kill circular stairs that are above themselves
				// FIX: skip dupliactes only between adjacent quads? this should help
				auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; };
				auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp);
				if (it == stairNodes.end()) {stairNodes.push_back(sn);}

			});

		}

		// reconstruct the z-value for each node using interpolation
		for (StairNode& sn : stairNodes) {

			// use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation
			const Point2 p(sn.x_cm, sn.y_cm);

			// the stair-quads centimeter position
			const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx];
			const Point3 p1 = quad.p1 * 100;
			const Point3 p2 = quad.p2 * 100;
			const Point3 p3 = quad.p3 * 100;
			const Point3 p4 = quad.p4 * 100;

			// get the z-value from one of the both triangles
//			int z_cm;
			float u,v,w;
			if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
				sn.z_cm = p1.z*u + p2.z*v + p3.z*w;
			} else {
				helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
				sn.z_cm = p1.z*u + p3.z*v + p4.z*w;
			}

		}

		const float stairAbsStart_m = floor->atHeight + stair->getParts().front().start.z;
		const float stairAbsEnd_m = floor->atHeight + stair->getParts().back().end.z;
		//const float stairHeight_m = stairAbsEnd_m - stairAbsStart_m;
		const float stairAbsStart_cm = std::round(stairAbsStart_m*100);
		const float stairAbsEnd_cm = std::round(stairAbsEnd_m*100);
		const float stairHeight_cm = stairAbsEnd_cm - stairAbsStart_cm;

		// this might lead to stairs the start slightly above the starting-floor
		// or ending slightly below the ending floor. this would lead to DISCONNECTION!
		// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
		{

			float minZ = +9999999;
			float maxZ = -9999999;
			for (const StairNode& sn : stairNodes) {
				if (sn.z_cm < minZ) {minZ = sn.z_cm;}
				if (sn.z_cm > maxZ) {maxZ = sn.z_cm;}
			}

			for (StairNode& sn : stairNodes) {
				const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ);					// current percentage from minZ to maxZ
				//sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100;		// apply percentage to floorStartZ <-> floorEndZ
				sn.z_cm = std::round(stairAbsStart_cm + zPercent * stairHeight_cm);			// apply percentage to floorStartZ <-> floorEndZ
			}

			// snap stair-nodes to nearby grid nodes, if possible
			if (tryImproveStairConnections) {
				for (StairNode& sn : stairNodes) {
					if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) )	{sn.z_cm = stairAbsStart_cm;}
					if (std::abs(sn.z_cm-stairAbsEnd_cm)   < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm)))	{sn.z_cm = stairAbsEnd_cm;}
				}
			}

			// stort all stair-nodes by z (ascending)
			const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;};
			std::sort(stairNodes.begin(), stairNodes.end(), comp);

			// get first and last stair note
			const bool end1OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.front().z_cm) != floorsAtHeight_cm.end();
			const bool end2OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(),  stairNodes.back().z_cm) != floorsAtHeight_cm.end();

			// be sure both are connected to a floor
			if (!end1OK || !end2OK) {
				throw Exception("stair's start or end is not directly connectable to a floor");
			}

		}



		// track z-positions of already-existing grid nodes we connected the stair to
		// if this list contains 2 distinctive values, the stair is successfully connected to starting and ending floor!
		std::unordered_set<float> connectedWithHeights;

		// add the new nodes to the grid
		// nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes!
		for (StairNode& sn : stairNodes) {

			// the to-be-added position
			GridPoint gp(sn.x_cm, sn.y_cm, sn.z_cm);

			// if a node is near an existing one (the floor above/below) use the existing one!
			// this ensures the stair is connected to the floor above and below
			if (grid.hasNodeFor(gp)) {

				sn.gridIdx = grid.getNodeFor(gp).getIdx();

				// remember the z-position of the already-existing grid-node we connected the stair to
				connectedWithHeights.insert(grid[sn.gridIdx].z_cm);

				// mark the node as stair-node
				//grid[sn.gridIdx].setType(GridNode::TYPE_STAIR);

			} else  {

				sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm));

				// check if there is a nearby floor-node to delete
				// -> remove nodes directly above/below the stair
				const int deleteDist_cm = 100;
				const float distToBelow = gp.z_cm - stairAbsStart_cm;//floor->getStartingZ()*100;
				const float distToAbove = stairAbsEnd_cm - gp.z_cm;//floor->getEndingZ()*100 - gp.z_cm;
				//if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
				if (distToBelow > 0 && distToBelow < deleteDist_cm) {
					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsStart_cm));//floor->getStartingZ()*100));
					if (n) {toDelete.push_back(n);}
				//} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
				}
				if (distToAbove > 0 && distToAbove < deleteDist_cm) {
					T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsEnd_cm));//floor->getEndingZ()*100));
					if (n) {toDelete.push_back(n);}
				}

				// mark the node as stair-node
				grid[sn.gridIdx].setType(GridNode::TYPE_STAIR);

			}

		}

		// sanity check
		// connectedWithHeights should contain 2 entries:
		// one for the starting floor
		// one for the ending floor
		// this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected
		Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");

		// now connect all new nodes with their neighbors
		// do not perform normal grid-connection but examine the nodes within the generated vector
		// this allows for some additional checks/criteria to be used
		for (const StairNode& sn1 : stairNodes) {
			T& n1 = (T&) grid[sn1.gridIdx];

			for (const StairNode& sn2 : stairNodes) {

				// node full?
				if (n1.fullyConnected()) {continue;}

				T& n2 = (T&) grid[sn2.gridIdx];

				// do not connect node with itself
				if (n2.getIdx() == n1.getIdx()) {continue;}

				if (n1.hasNeighbor(n2.getIdx())) {continue;}

				// connect adjacent stair-nodes
				// but only if their stair-parts are also adjacent
				// this addresses several error-situations with round stairs (end connected to the start) , ...
				const int dx = sn1.x_cm - sn2.x_cm;
				const int dy = sn1.y_cm - sn2.y_cm;
				const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx;

				// connect
				if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) {
					grid.connectUniDir(n1, n2);
				}

			}


		}



	}


	void finalize() {

		//std::cout << "stairs.finalize() crashes!" << std::endl;
		//return;

		// delete all pending nodes and perform a cleanup
		if (!toDelete.empty()) {
			for (T* n : toDelete) {grid.remove(*n);}
			toDelete.clear();
			grid.cleanup();
		}

	}



};

#endif // STAIRS_H