/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef GRIDFACTORY3_H
#define GRIDFACTORY3_H

#include "../../Grid.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "HelperPoly3.h"
#include <unordered_set>

#if (GRID_MODE == GM_BOX)

#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) {	for (int x = x1; x <= x2; x += gs_cm) {

#elif (GRID_MODE == GM_HOBEYCOMB)

#define GF3_ITER_XY\
for (int y = y1; y <= y2; y += gs_cm) {\
	const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\
	for (int x = x1-xx; x <= x2; x += gs_cm) {

#endif

template <typename Node> class GridFactory3 {

private:


	Grid<Node>& grid;
	const int gs_cm;

	struct NewNode {
		GridPoint pos;
		int type;
		NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;}
		bool operator == (const NewNode& o) const {return o.pos == pos;}
	};

public:

	GridFactory3(Grid<Node>& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) {

	}

	void build(const Floorplan::IndoorMap* map) {

		std::vector<NewNode> add;
		std::vector<NewNode> rem;

		for (const Floorplan::Floor* floor : map->floors) {

//			for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {

//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
//				if (poly->method == Floorplan::OutlineMethod::ADD) {
//					add.insert(add.end(), pts.begin(), pts.end());
//				} else {
//					rem.insert(rem.end(), pts.begin(), pts.end());
//				}

//			}

			const std::vector<NewNode> pts = getPointsOn(floor);
			add.insert(add.end(), pts.begin(), pts.end());

			for (const Floorplan::Stair* stair : floor->stairs) {
				std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), floor);
				const std::vector<NewNode> pts = getPointsOn(floor, quads);
				add.insert(add.end(), pts.begin(), pts.end());
			}


		}

		for (const NewNode& nn : add) {
			auto it = std::find(rem.begin(), rem.end(), nn);
			if (it == rem.end()) {
				if (!grid.hasNodeFor(nn.pos)) {
					Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm);
					n.setType(nn.type);
					grid.add(n);
				}
			}
		}

		connect(map);
		removeIsolatedNodes();

	}

	bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) {

		Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy());

		for (Floorplan::Floor* floor : map->floors) {

			if (n1.inMeter().z != floor->atHeight) {continue;}
			if (n2.inMeter().z != floor->atHeight) {continue;}

			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
				if (line) {
					const std::vector<Line2> lines = getThickLines(line);
					for (const Line2& lObs : lines) {
						if (lObs.getSegmentIntersection(lNodes)) {
							return true;
						}
					}
				}
			}
		}

		return false;

	}

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

	void connect(const Floorplan::IndoorMap* map) {

		for (Node& n1 : grid) {
			for (Node& n2 : grid) {

				if (n1 == n2) {continue;}


				// stair with floor
				if (
						(n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) ||
						(n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR)
					) {

					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);
					if (distxy > 0 && distxy < gs_cm * 1.2 / 100.0f && distz_cm < gs_cm) {		// [1.85]
						if (n1.fullyConnected()) {continue;}
						if (n2.fullyConnected()) {continue;}
						grid.connectUniDir(n1, n2);
					}

				// floor with floor
				} else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) {
					if (n1.getDistanceInCM(n2) < gs_cm * 1.2  && !isBlocked(map, n1, n2)) {		// [1.2 | 1.845]
						if (n1.fullyConnected()) {continue;}
						if (n2.fullyConnected()) {continue;}
						grid.connectUniDir(n1, n2);
					}

				// stair with stair
				} else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) {

					const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy());
					const float distz_cm = std::abs(n1.z_cm - n2.z_cm);

//					if (n1.getDistanceInCM(n2) < gs_cm * 1.45  && !isBlocked(map, n1, n2)) {
					if (distxy < gs_cm * 1.2 / 100.0f && distz_cm <= gs_cm) {		// [1.845]
						if (n1.fullyConnected()) {continue;}
						if (n2.fullyConnected()) {continue;}
						grid.connectUniDir(n1, n2);
					}

				}


//				if (n1.getDistanceInCM(n2) < gs_cm * 1.7  && !isBlocked(map, n1, n2)) {
//					if (n1.fullyConnected()) {continue;}
//					if (n2.fullyConnected()) {continue;}
//					grid.connectUniDir(n1, n2);
//				}

			}
		}

	}
















	/** recursively get all connected nodes and add them to the set */
	void getConnected(Node& 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);
			Node& next = grid[nextIdx];

			// get all his (unprocessed) neighbors and add them to the region
			for (const Node& n2 : grid.neighbors(next)) {
				if (visited.find(n2.getIdx()) == visited.end()) {
					toVisit.insert(n2.getIdx());
				}
			}

		}

	}

	void removeIsolatedNodes() {

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

		// try to start at the first stair
		for (Node& 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(Node& 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 (Node& 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();

	}


//	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) {

//		std::vector<NewNode> res;

//		BBox2 bbox;
//		for (Point2 pt : poly.poly.points) {bbox.add(pt);}

//		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
//		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;

//		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
//		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;

//		int z = floor->atHeight * 100;

//		for (int y = y1; y <= y2; y += gs_cm) {
//			for (int x = x1; x <= x2; x += gs_cm) {

//				GridPoint gp(x, y, z);
//				int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR;
//				res.push_back(NewNode(gp, type));

//			}
//		}

//		return res;

//	}



	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor) {

		std::vector<NewNode> res;

		BBox2 bbox;
		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
			for (Point2 pt : poly->poly.points) {bbox.add(pt);}
		}

		int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
		int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;

		int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
		int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;

		int z = floor->atHeight * 100;

		struct Combo {
			HelperPoly3 poly;
			const Floorplan::FloorOutlinePolygon* orig;
			Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;}
		};

		std::vector<Combo> polygons;
		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
			HelperPoly3 pol(*poly);
			polygons.push_back(Combo(pol, poly));
		}

		GF3_ITER_XY

				int type = GridNode::TYPE_FLOOR;
				bool remove = false;
				bool add = false;

				for (const Combo& c : polygons) {
					if (c.poly.contains(Point2(x,y))) {
						if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;}
						if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;}
						if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;}
					}
				}

				if (add && !remove) {
					GridPoint gp(x, y, z);
					res.push_back(NewNode(gp, type));
				}

			}
		}

		return res;

	}



	//

	//				const std::vector<NewNode> pts = getPointsOn(floor, *poly);
	//				if (poly->method == Floorplan::OutlineMethod::ADD) {
	//					add.insert(add.end(), pts.begin(), pts.end());
	//				} else {
	//					rem.insert(rem.end(), pts.begin(), pts.end());
	//				}

	//			}

	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
		const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
		double d00 = dot(v0, v0);
		double d01 = dot(v0, v1);
		double d11 = dot(v1, v1);
		double d20 = dot(v2, v0);
		double d21 = dot(v2, v1);
		double denom = d00 * d11 - d01 * d01;
		v = (d11 * d20 - d01 * d21) / denom;
		w = (d00 * d21 - d01 * d20) / denom;
		u = 1.0f - v - w;
		return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0);
	}

//	void isBlocked(const GridPoint& gp) {
//		for (Floorplan::Floor* floor : map->floors) {
//			for (Floorplan::FloorObstacle* obs : floor->obstacles) {
//				Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
//				if (line) {
//					line->
//				}
//			}
//		}
//	}

	std::vector<NewNode> getPointsOn(const Floorplan::Floor* floor, const std::vector<Floorplan::Quad3>& quads) {

		std::vector<NewNode> res;

		// whole stair
		BBox3 bboxStair;
		for (const Floorplan::Quad3& quad : quads) {
			bboxStair.add(quad.p1);
			bboxStair.add(quad.p2);
			bboxStair.add(quad.p3);
			bboxStair.add(quad.p4);
		}

		// stair's starting and ending z (must be connected to a floor)
		//int z1 = grid.snapZ( (floor->atHeight) * 100 );
		//
		int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 );

		// one quad
		for (const Floorplan::Quad3& quad : quads) {

			BBox3 bbox;
			bbox.add(quad.p1);
			bbox.add(quad.p2);
			bbox.add(quad.p3);
			bbox.add(quad.p4);


			int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm;
			int x2 =  std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm;

			int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm;
			int y2 =  std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm;

			//int zFloor = floor->atHeight * 100;

//			for (int y = y1; y <= y2; y += gs_cm) {
//				const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);
//				for (int x = x1-xx; x <= x2; x += gs_cm) {

			GF3_ITER_XY

					int z = 0;
					Point2 p(x/100.0f, y/100.0f);

					float u,v,w;
					if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) {
						z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100;
					} else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) {
						z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
					} else {
						// outside of the quad -> skip
						//z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100;
						continue;

						//z = zFloor + (
						//		(quad.p1.z*u + quad.p2.z*v + quad.p3.z*w)
						//	) * 100;

					}

					//z = grid.snapZ(z);

					const GridPoint gp(x, y, z);
					const int type = GridNode::TYPE_STAIR;
					res.push_back(NewNode(gp, type));

				}
			}

		}

		// scale to ensure starting at floor, and ending at floor

		return res;

	}

};

#endif // GRIDFACTORY3_H