#ifndef GRID_FACTORY_HELPER_H
#define GRID_FACTORY_HELPER_H

#include "../../../geo/Point2.h"
#include "../../../geo/Point3.h"
#include "../../../geo/BBox2.h"
#include "../../../geo/BBox3.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "../../../grid/Grid.h"

/** helper class for polygon methods */
struct HelperPoly {

	BBox2 bbox_cm;
	std::vector<Point2> points_cm;

	/** empty ctor */
	HelperPoly() {
		;
	}

	/** ctor from floorplan-polygon */
	HelperPoly(const Floorplan::FloorOutlinePolygon& poly) {
		for (Point2 p : poly.poly.points) { add(p * 100); }
	}

	/** ctor from floorplan-quad */
	HelperPoly(const Floorplan::Quad3& quad) {
		add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
	}

	void add(const Point2 p) {
		points_cm.push_back(p);
		bbox_cm.add(p);
	}

	void add(const Point3& p) {
		points_cm.push_back(p.xy());
		bbox_cm.add(p.xy());
	}

	/** does the polygon contain the given point (in cm)? */
	bool contains(const Point2 p_cm) const {

		// not within bbox? -> not within polygon
		if (!bbox_cm.contains(p_cm)) {return false;}

		// ensure the point is at least a bit outside of the polygon
		const float x1_cm = bbox_cm.getMin().x - 17.71920;
		const float y1_cm = bbox_cm.getMin().y - 23.10923891;

		// construct line between point outside of the polygon and the point in question
		const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y);

		// determine the number of intersections
		int hits = 0;
		const int cnt = points_cm.size();
		for (int i = 0; i < cnt; ++i) {
			const Point2 p1 = points_cm[(i+0)%cnt];
			const Point2 p2 = points_cm[(i+1)%cnt];
			const Line2 l12(p1, p2);
			if (l12.getSegmentIntersection(l)) {++hits;}
		}

		// inside or outside?
		return ((hits % 2) == 1);

	}

};













template <typename T> class Helper {

private:

	Grid<T>& grid;

public:

	/** ctor */
	Helper(Grid<T>& grid) : grid(grid) {

	}

	/** connect the given node to all its neighbors */
	void connectToNeighbors(T& n1) {

		const int gs_cm = grid.getGridSize_cm();

		for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
			for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {

				// skip the node itself
				if (x == 0 && y == 0) {continue;}

				// try to find a matching neighbor
				const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm);
				const T* n2 = grid.getNodePtrFor(gp);
				if (!n2) {continue;}

				// connect
				if (n1.hasNeighbor(n2->getIdx())) {continue;}
				grid.connectUniDir(n1, *n2);

			}
		}

	}



	int gridSize() const {
		return grid.getGridSize_cm();;
	}

	float align(const float val) {
		const float gridSize_cm = gridSize();
		return std::round(val/gridSize_cm) * gridSize_cm;
	}

	float alignF(const float val) {
		const float gridSize_cm = gridSize();
		return std::floor(val/gridSize_cm) * gridSize_cm;
	}

	float alignC(const float val) {
		const float gridSize_cm = gridSize();
		return std::ceil(val/gridSize_cm) * gridSize_cm;
	}

	Point3 align(const Point3 p) {
		return Point3( align(p.x), align(p.y), (p.z) );		// TODO: align z or not?
	}

	float min(float a, float b, float c, float d) {
		if (a < b && a < c && a < d) {return a;}
		if (b < a && b < c && b < d) {return b;}
		if (c < a && c < b && c < d) {return c;}
		if (d < a && d < b && d < c) {return d;}
	}
	float max(float a, float b, float c, float d) {
		if (a > b && a > c && a > d) {return a;}
		if (b > a && b > c && b > d) {return b;}
		if (c > a && c > b && c > d) {return c;}
		if (d > a && d > b && d > c) {return d;}
	}

	void clamp(float& val, const int min, const int max) {
		while (val < min) {val += gridSize();}
		while (val > max) {val -= gridSize();}
	}


	void limit(Floorplan::Quad3& q2, const Floorplan::Quad3& q1) {
		const float x1 = min(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
		const float x2 = max(q1.p1.x, q1.p2.x, q1.p3.x, q1.p4.x);
		const float y1 = min(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
		const float y2 = max(q1.p1.y, q1.p2.y, q1.p3.y, q1.p4.y);
		const float z1 = min(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
		const float z2 = max(q1.p1.z, q1.p2.z, q1.p3.z, q1.p4.z);
//		clamp(q2.p1.x, x1, x2);	clamp(q2.p2.x, x1, x2); clamp(q2.p3.x, x1, x2); clamp(q2.p4.x, x1, x2);
//		clamp(q2.p1.y, y1, y2);	clamp(q2.p2.y, y1, y2); clamp(q2.p3.y, y1, y2); clamp(q2.p4.y, y1, y2);
		clamp(q2.p1.z, z1, z2);	clamp(q2.p2.z, z1, z2); clamp(q2.p3.z, z1, z2); clamp(q2.p4.z, z1, z2);
	}

	Floorplan::Quad3 align(const Floorplan::Quad3& q) {
		Floorplan::Quad3 q2 = Floorplan::Quad3(align(q.p1), align(q.p2), align(q.p3), align(q.p4));
		//limit(q2, q);
		return q2;
	}





	static float dot(const Point2 a, const Point2 b) {
		return a.x * b.x + a.y * b.y;
	}

	static float area(const Point2 a, const Point2 b, const Point2 c) {
		//Point2 ab = b-a;
		//Point2 ac = c-a;
		//return 0.5f * std::sqrt(dot(ab,ab)*dot(ac,ac) - dot(ab,ac)*dot(ab,ac));
		Point2 p1 = b-a;
		Point2 p2 = c-a;
		return std::abs(p1.x*p2.y - p2.x*p1.y) * 0.5;
	}

	static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
		Point2 v0 = b - a, v1 = c - a, v2 = p - a;
		float d00 = dot(v0, v0);
		float d01 = dot(v0, v1);
		float d11 = dot(v1, v1);
		float d20 = dot(v2, v0);
		float d21 = dot(v2, v1);
		float 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);
	}



};

#endif // GRID_FACTORY_HELPER_H