#ifndef STAIRS_H
#define STAIRS_H

#include <vector>
#include <set>

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

#include <fstream>



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;


	std::ofstream outStairs;
	std::ofstream outDelete;


private:

//	struct Entry {
//		GridPoint pos;
//		std::vector<int> part;
//		int idx;
//		Entry(const GridPoint pos, int part) : pos(pos) {
//			this->part.push_back(part);
//		}
//	};

	/** 2D integer point */
	struct XY {
		int x;
		int y;
		XY() : x(-1), y(-1) {;}
		XY(const int x, const int y) : x(x), y(y) {;}
		bool operator == (const XY& o) const {return (x == o.x) && (y == o.y);}
	};


	/** 3D integer point */
	struct XYZ {
		int x;
		int y;
		int z;
		XYZ() : x(-1), y(-1), z(-1) {;}
		XYZ(const int x, const int y, const int z) : x(x), y(y), z(z) {;}
		bool operator == (const XYZ& o) const {return (x == o.x) && (y == o.y) && (z == o.z);}
	};

	/** and intermediate grid node */
	struct Intermediate {
		XY xy;
		T node;
		int idx;
		std::vector<XY> con;
		Intermediate(const XY xy, const T node) : xy(xy),node(node), idx(-1) {;}
	};


public:

	/** ctor */
	Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
		outStairs.open("/tmp/quads.dat");
		outDelete.open("/tmp/delete.dat");
	}

	~Stairs() {
		finalize();
		outStairs.close();
		outDelete.close();
	}



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

		const int gs_cm = grid.getGridSize_cm();

		std::vector<Intermediate> stairNodes;

		// process each part
		int partIdx = 0;

		const std::vector<Floorplan::StairPart> parts = stair->getParts();
		const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);


		for (int i = 0; i < (int)parts.size(); ++i) {

			//const Floorplan::StairPart& part = parts[i];
			const Floorplan::Quad3& quad = helper.align(quads[i]*100);

			const Point3 p1 = quad.p1;
			const Point3 p2 = quad.p2;
			const Point3 p3 = quad.p3;
			const Point3 p4 = quad.p4;

			outStairs << p1.x << " " << p1.y << " " << p1.z << "\n";
			outStairs << p2.x << " " << p2.y << " " << p2.z << "\n";
			outStairs << p3.x << " " << p3.y << " " << p3.z << "\n";
			outStairs << p4.x << " " << p4.y << " " << p4.z << "\n";
			outStairs << p1.x << " " << p1.y << " " << p1.z << "\n\n\n";


			HelperPoly poly; poly.add(p1.xy()); poly.add(p2.xy()); poly.add(p3.xy()); poly.add(p4.xy());

			const int x1 = helper.align(poly.bbox_cm.getMin().x);		// ALIGNF?
			const int y1 = helper.align(poly.bbox_cm.getMin().y);
			const int x2 = helper.align(poly.bbox_cm.getMax().x);		// ALIGNC?
			const int y2 = helper.align(poly.bbox_cm.getMax().y);

			std::vector<XYZ> points;

			// get all points that are part of the stair-part-polygon
			for (int x = x1; x <= x2; x+=gs_cm) {
				for (int y = y1; y <= y2; y+=gs_cm) {
					if (!poly.contains( Point2(x,y) )) {continue;}
					points.push_back(XYZ(x,y,-1));
					if (y < 500) {
						int i = 0; (void) i;
					}
				}
			}

			// now determine each corresponding z-coordinate
			for (XYZ& xy : points) {

				const Point2 p(xy.x, xy.y);
				float u,v,w;

				if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
					xy.z = p1.z*u + p2.z*v + p3.z*w;
				} else {
					helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
					xy.z = p1.z*u + p3.z*v + p4.z*w;
				}

			}

			// stair start, ensure min snaps to the floor
			if (partIdx == 0) {
				const float minz_cm = minZ(points);
				const float maxz_cm = maxZ(points);
				const int mz = floor->atHeight * 100;
				if (minz_cm != mz) {
					for (XYZ& xyz : points) {
						const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
						xyz.z = mz + percent * (maxz_cm - mz);
					}
				}
			}

			// stair end, ensure max snaps to the floor
			if (partIdx == (int) parts.size() - 1) {
				const float minz_cm = minZ(points);
				const float maxz_cm = maxZ(points);
				const int mz = (floor->atHeight+floor->height) * 100;
				if (maxz_cm != mz) {
					for (XYZ& xyz : points) {
						const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
						xyz.z = minz_cm + percent * (mz - minz_cm);
					}
				}
			}



			for (XYZ& xy : points) {

				// construct a grid-node
				T node(xy.x, xy.y, xy.z);

				// construct an intermediate-node containing all desired connections
				Intermediate iNode(XY(xy.x, xy.y), node);
				for (int ox = -1; ox <= +1; ++ox) {
					for (int oy = -1; oy <= +1; ++oy) {
						if (ox == 0 && oy == 0) {continue;}
						const int nx = xy.x + ox * gs_cm;
						const int ny = xy.y + oy * gs_cm;
						iNode.con.push_back( XY(nx, ny) );
					}
				}
				stairNodes.push_back(iNode);

			}


			++partIdx;

		}


		// add all stair nodes or replace them with already existing ones
		for (Intermediate& iNode : stairNodes) {

			// nearer than minDiff? -> use existing node
			const float minDiff = gs_cm * 0.49;

			// nearer than maxDiff? -> delete as this one is unreachable below the stair
			// to prevent errors (e.g. a platform at 1.0 meter) we use 0.98 meter as deleting threshold
			// otherwise we get problems in some places
			const float maxDiff = 98;

			// distance between the stair node and the floor above / below
			const float zDiff1 = std::abs(iNode.node.z_cm - 100*floor->getStartingZ());
			const float zDiff2 = std::abs(iNode.node.z_cm - 100*floor->getEndingZ());

			iNode.idx = -1;

			// try to connect the stair-node to the floor below
			if (zDiff1 < minDiff) {
				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
				if (n2) {
					iNode.idx = n2->getIdx();
				} else {
					iNode.idx = grid.add(iNode.node);				// add a new one
					helper.connectToNeighbors(grid[iNode.idx]);		// and connect to the floor's grid nodes
				}

			// try to connect the stair-node to the floor above
			} else if (zDiff2 < minDiff) {
				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
				if (n2) {
					iNode.idx = n2->getIdx();
				} else {
					iNode.idx = grid.add(iNode.node);				// add a new one
					helper.connectToNeighbors(grid[iNode.idx]);		// and connect to the floor's grid nodes
				}

			// remove nodes directly below the stair
			} else if (zDiff1 < maxDiff) {
				T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
				outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
				if (n2->y_cm < 500) {
					int i = 0; (void) i;
				}
				if (n2) {toDelete.push_back(n2);}

			// remove nodes directly above the stair
			} else if (zDiff2 < maxDiff) {
				T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
				outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
				if (n2->y_cm < 500) {
					int i = 0; (void) i;
				}
				if (n2) {toDelete.push_back(n2);}
			}

			// no matching node found -> add a new one to the grid
			if (iNode.idx == -1) {
				const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, iNode.node.z_cm));
				iNode.idx = (n2) ? (n2->getIdx()) : (grid.add(iNode.node));
			}

			// add semantic information
			grid[iNode.idx].setType(GridNode::TYPE_STAIR);

		}

		// connect to-be-connected stair-nodes (each node with its [previously configured] neighbors)
		for (const Intermediate& t1 : stairNodes) {
			for (const XY& connectTo : t1.con) {
				for (const Intermediate& t2 : stairNodes) {
					if (connectTo == t2.xy) {
						T& n1 = grid[t1.idx];				// use the nodes already added to the grid
						T& n2 = grid[t2.idx];				// use the nodes already added to the grid
						if (n1.hasNeighbor(n2.getIdx()))	{continue;}
						if (n1.getIdx() == n2.getIdx())		{continue;}
						if (n1.getNumNeighbors() >= 10)		{continue;}
						grid.connectUniDir(n1, n2);
					}
				}
			}
		}

		// finalize after ALL stairs were added. much faster and should be safe!
		//finalize();


	}

	void finalize() {

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

	}

	static float minZ(const std::vector<XYZ>& points) {
		auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
		auto it = std::min_element(points.begin(), points.end(), comp);
		return it->z;
	}

	static float maxZ(const std::vector<XYZ>& points) {
		auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
		auto it = std::max_element(points.begin(), points.end(), comp);
		return it->z;
	}

};

#endif // STAIRS_H