#ifndef FLOORPLAN_3D_WALLSVIACUTTEDQUADS_H
#define FLOORPLAN_3D_WALLSVIACUTTEDQUADS_H

#include "../../geo/Line2.h"
#include "../../geo/Polygon2.h"
#include "../../geo/GPCPolygon2.h"

//#include "Walls.h"
#include "misc.h"

#include <functional>
#include <iostream>

namespace Floorplan3D {

	/**
	 * interpret walls als quads (polygons)
	 * intersect them with each other to prevent overlaps
	 */
	class WallsViaCuttedQuads {

	private:

		struct Wall {

			/** algorithms set error flag here */
			bool error = false;

			/** original line from floorplan */
			const Floorplan::FloorObstacleWall* line;

			/** outlines after applying thickness */
			Line2 l1;
			Line2 l2;

			Wall(const Floorplan::FloorObstacleWall* line) : line(line) {

				const Point2 from = line->from;
				const Point2 to = line->to;
				const Point2 dir = (to-from).normalized();
				const Point2 dirP = dir.perpendicular();
				const float w = line->thickness_m;
				const float w2 = w/2;

				const Point2 p1 = from + dirP * w2;
				const Point2 p2 = from - dirP * w2;
				const Point2 p3 = to - dirP * w2;
				const Point2 p4 = to + dirP * w2;

				l1 = Line2(p1, p4);
				l2 = Line2(p2, p3);

			}

			/** get points for CCW wall outline (p1->p2->p3->p4) */
			Point2 getP1() const {return l1.p1;}
			Point2 getP2() const {return l1.p2;}
			Point2 getP3() const {return l2.p2;}
			Point2 getP4() const {return l2.p1;}

			Point2& getP1() {return l1.p1;}
			Point2& getP2() {return l1.p2;}
			Point2& getP3() {return l2.p2;}
			Point2& getP4() {return l2.p1;}

			struct CutRes {
				Point2 p;
				Line2* l1;	// affected line within this wall
				Line2* l2;	// affected line within the other wall
				CutRes(Point2 p, Line2* l1, Line2* l2) : p(p), l1(l1), l2(l2) {;}
			};

			/** get all intersecting points between the two walls */
			std::vector<CutRes> getIntersections(Wall& o, bool limit = true) {
				std::vector<CutRes> res;
				Point2 p;
				if (intersects(l1, o.l1, limit, p)) {res.push_back(CutRes(p,&l1, &o.l1));}
				if (intersects(l1, o.l2, limit, p)) {res.push_back(CutRes(p,&l1, &o.l2));}
				if (intersects(l2, o.l1, limit, p)) {res.push_back(CutRes(p,&l2, &o.l1));}
				if (intersects(l2, o.l2, limit, p)) {res.push_back(CutRes(p,&l2, &o.l2));}
				return res;
			}

			/** is this wall directly attached to the given wall? */
			bool directlyConnectedTo(const Wall& o) const {
				const float d = 0.001;
				return
				    (line->from.eq(	o.line->from, d)) ||
				    (line->to.eq(	o.line->from, d)) ||
				    (line->from.eq(	o.line->to, d)) ||
				    (line->to.eq(	o.line->to, d));
			}

			/** does this wall end within the given wall? */
			bool endsWithin(const Wall& o) const {
				return o.containsPoint(line->from) || o.containsPoint(line->to);
			}

			/** does this wall contain the given point */
			bool containsPoint(const Point2 p) const {
				return polygonContainsPoint({getP1(), getP2(), getP3(), getP4()}, p);
			}

		};

		void cropLine(Line2* l, Point2 p) {

			// determine which line-end to crop
			if (p.getDistance(l->p1) < p.getDistance(l->p2)) {
				l->p1 = p;
			} else {
				l->p2 = p;
			}

		}

		void cutInMiddle(Point2& pa1, Point2& pa2, Point2& pb1, Point2& pb2) {

			// line from pa1->pa2, and pb1->pb2
			// intersection expected near pa2|pb1

			Line2 l1(pa1, pa2);	l1 = l1.longerAtEnd(10);
			Line2 l2(pb1, pb2);	l2 = l2.longerAtStart(10);

			Point2 pi;
			if (intersects(l1, l2, true, pi)) {
				pa2 = pi;			// replace end of line1
				pb1 = pi;			// replace start of line2
			}

		}

		std::vector<Wall> walls;
		const Floorplan::Floor* floor;
		std::vector<Obstacle3D> obs;

	public:


		void clear() {
			walls.clear();
		}

		void add(const Floorplan::Floor* f, const Floorplan::FloorObstacleWall* fow) {
			if (fow->type != Floorplan::ObstacleType::WALL) {return;}
			this->floor = f;
			walls.push_back(Wall(fow));
		}

		virtual const std::vector<Obstacle3D>& get() {
			std::vector<Wall> tmp = walls;
			tmp = cutConnected(tmp);
			tmp = cutProtruding(tmp);
			obs = toObstacle(tmp, floor);
			return obs;
		}

	private:

		/** convert all walls to obstacles */
		std::vector<Obstacle3D> toObstacle(const std::vector<Wall>& walls, const Floorplan::Floor* f) {
			std::vector<Obstacle3D> res;
			for (const Wall& w : walls) {
				res.push_back(toObstacle(w, f));
			}
			return res;
		}


		/** convert one wall into an obstacle */
		Obstacle3D toObstacle(const Wall& wall, const Floorplan::Floor* f) {

			FloorPos fp(f);

			Obstacle3D::Type type = (wall.error) ? (Obstacle3D::Type::ERROR) : (getType(wall.line));
			Obstacle3D obs(type, wall.line->material);

			const float z1 = fp.z1;
			const float z2 = (wall.line->height_m > 0) ? (fp.z1 + wall.line->height_m) : (fp.z2);

//			const Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
//			const Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
//			const Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
//			const Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);

//			const Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
//			const Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
//			const Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
//			const Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);

//			obs.addQuad(p1, p2, p2u, p1u);
//			obs.addQuad(p2, p3, p3u, p2u);
//			obs.addQuad(p3, p4, p4u, p3u);
//			obs.addQuad(p4, p1, p1u, p4u);

//			obs.addQuad(p1u, p2u, p3u, p4u);
//			obs.addQuad(p4, p3, p2, p1);

//			obs.reverseFaces();

//			return obs;

			Point3 p0 = Point3(wall.line->from.x, wall.line->from.y, z1);

			Point3 p1 = Point3(wall.getP1().x, wall.getP1().y, z1);
			Point3 p2 = Point3(wall.getP2().x, wall.getP2().y, z1);
			Point3 p3 = Point3(wall.getP3().x, wall.getP3().y, z1);
			Point3 p4 = Point3(wall.getP4().x, wall.getP4().y, z1);

			Point3 p1u = Point3(wall.getP1().x, wall.getP1().y, z2);
			Point3 p2u = Point3(wall.getP2().x, wall.getP2().y, z2);
			Point3 p3u = Point3(wall.getP3().x, wall.getP3().y, z2);
			Point3 p4u = Point3(wall.getP4().x, wall.getP4().y, z2);

			Point3 o = p0;
			float t = wall.line->thickness_m / 2;

			const Point3 o1;// = p1;
			const Point3 o2;// = p4;
			const float a = std::atan2(wall.getP2().y - wall.getP1().y, wall.getP2().x - wall.getP1().x);

			auto flatten = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();};
			//auto flatten2 = [a,o] (const Point3 p) {return (p - o).rotZ(-a).xz();};

			auto unFlattenFront =	[o,t,a] (const Point3 p) {return Point3(p.x, +t, p.y).rotZ(a)+o;};
			auto unFlattenBack =	[o,t,a] (const Point3 p) {return Point3(p.x, -t, p.y).rotZ(a)+o;};

			auto unFlattenFront2 =	[o,t,a] (const Point2 p) {return Point3(p.x, +t, p.y).rotZ(a)+o;};
			auto unFlattenBack2 =	[o,t,a] (const Point2 p) {return Point3(p.x, -t, p.y).rotZ(a)+o;};

			const Point2 fp1 = flatten(p1);
			const Point2 fp2 = flatten(p2);
			const Point2 fp3 = flatten(p3);
			const Point2 fp4 = flatten(p4);
			const Point2 fp1u = flatten(p1u);
			const Point2 fp2u = flatten(p2u);
			const Point2 fp3u = flatten(p3u);
			const Point2 fp4u = flatten(p4u);


			Polygon2 front;
			front.add({fp1, fp2, fp2u, fp1u});

			GPCPolygon2 gpFront;
			gpFront.add(front);

			Polygon2 back;
			back.add({fp3, fp4, fp4u, fp3u});

			GPCPolygon2 gpBack;
			gpBack.add(back);

			// sort doors by their position within the wall (first comes first)
			std::vector<Floorplan::FloorObstacleWallDoor*> doors = wall.line->doors;
			auto compDoors = [] (const Floorplan::FloorObstacleWallDoor* d1, Floorplan::FloorObstacleWallDoor* d2) {
				return d1->atLinePos < d2->atLinePos;
			};
			std::sort(doors.begin(), doors.end(), compDoors);


			TriangleStrip strip;

			strip.add(p1);
			strip.add(p4);

			for (const Floorplan::FloorObstacleWallDoor* door : doors) {

				const Point2 pds = door->getStart(wall.line);
				const Point2 pde = door->getEnd(wall.line);

				const Point3  dp1(pds.x, pds.y, z1);
				const Point3  dp2(pde.x, pde.y, z1);
				const Point3 dp2u(pde.x, pde.y, z1+door->height);
				const Point3 dp1u(pds.x, pds.y, z1+door->height);

				Polygon2 pDoor;
				pDoor.add(flatten(dp1));
				pDoor.add(flatten(dp2));
				pDoor.add(flatten(dp2u));
				pDoor.add(flatten(dp1u));

				// wall cutout (front/back)
				gpFront.remove(pDoor);
				gpBack.remove(pDoor);

				// 3D framing
				strip.add(unFlattenFront2(pDoor[0]));
				strip.add(unFlattenBack2(pDoor[0]));

				strip.add(unFlattenFront2(pDoor[3]));
				strip.add(unFlattenBack2(pDoor[3]));

				strip.add(unFlattenFront2(pDoor[2]));
				strip.add(unFlattenBack2(pDoor[2]));

				strip.add(unFlattenFront2(pDoor[1]));
				strip.add(unFlattenBack2(pDoor[1]));

			}

			strip.add(p2);
			strip.add(p3);
			strip.add(p2u);
			strip.add(p3u);
			strip.add(p1u);
			strip.add(p4u);
			strip.add(p1);
			strip.add(p4);

			for (Triangle3 t : strip.toTriangles()) {
				t.reverse();
				obs.triangles.push_back(t);
			}

			// process all windows within the wall

			for (const Floorplan::FloorObstacleWallWindow* win : wall.line->windows) {

				const Point2 pws = win->getStart(wall.line);
				const Point2 pwe = win->getEnd(wall.line);

				const float wz1 = z1 + win->startsAtHeight;
				const float wz2 = z1 + win->startsAtHeight + win->height;

				const Point3  dp1(pws.x, pws.y, wz1);
				const Point3  dp2(pwe.x, pwe.y, wz1);
				const Point3 dp2u(pwe.x, pwe.y, wz2);
				const Point3 dp1u(pws.x, pws.y, wz2);

				Polygon2 pWindow;
				pWindow.add(flatten(dp1));
				pWindow.add(flatten(dp2));
				pWindow.add(flatten(dp2u));
				pWindow.add(flatten(dp1u));

				// wall cutout (front/back)
				gpFront.remove(pWindow);
				gpBack.remove(pWindow);

				// 3D framing
				TriangleStrip strip;

				strip.add(unFlattenFront2(pWindow[0]));
				strip.add(unFlattenBack2(pWindow[0]));

				strip.add(unFlattenFront2(pWindow[3]));
				strip.add(unFlattenBack2(pWindow[3]));

				strip.add(unFlattenFront2(pWindow[2]));
				strip.add(unFlattenBack2(pWindow[2]));

				strip.add(unFlattenFront2(pWindow[1]));
				strip.add(unFlattenBack2(pWindow[1]));

				strip.add(unFlattenFront2(pWindow[0]));
				strip.add(unFlattenBack2(pWindow[0]));

				for (Triangle3 t : strip.toTriangles()) {
					t.reverse();
					obs.triangles.push_back(t);
				}

			}

			// Frontseite triangulieren
			std::vector<Triangle3> triasFront = gpFront.getTriangles();

			for (Triangle3 tria : triasFront) {
				fixTria(tria, unFlattenFront);
				tria.reverse();
				obs.triangles.push_back(tria);
			}

			// Rückseite triangulieren
			std::vector<Triangle3> triasBack = gpBack.getTriangles();

			for (Triangle3 tria : triasBack) {
				fixTria(tria, unFlattenBack);
				obs.triangles.push_back(tria);
			}


//			for (const Floorplan::FloorObstacleWallDoor* door : wall.line->doors) {
//				const Point2 p1 = door->getStart(wall.line);
//				const Point2 p2 = door->getEnd(wall.line);

//			}

			return obs;

		}

		void fixTria(Triangle3& t, std::function<Point3(Point3)> func) {
			t.p1 = func(t.p1);
			t.p2 = func(t.p2);
			t.p3 = func(t.p3);
		}

		/** cut off walls ending within another wall */
		std::vector<Wall> cutProtruding(std::vector<Wall> walls) {

			// if one wall ends within another one cut it off

			for (size_t i = 0; i < walls.size(); ++i) {
				Wall& w1 = walls[i];

				for (size_t j = i+1; j < walls.size(); ++j) {
					Wall& w2 = walls[j];

					if (i == j) {continue;}

					// if the two walls are directly connected (share one node) -> ignore this case here!
					if (w1.directlyConnectedTo(w2)) {continue;}

					// not the case we are looking for?
					if (!w1.endsWithin(w2) && !w2.endsWithin(w1)) {continue;}

					// get all intersection points between the two walls
					std::vector<Wall::CutRes> isects = w1.getIntersections(w2);

					// this should be 0 (no intersections) or 2 (one for each outline)
					if (!isects.empty() && isects.size() != 2) {
						w1.error = true;
						w2.error = true;
						std::cout << "detected strange wall intersection" << std::endl;
					}

					int cut = 0;

					// check the (2) detected intersections
					for (const auto isect : isects) {

						// if one of the line-ends p1/p2 from wall1 ends within wall2, crop it by setting it to the intersection
						if (w2.containsPoint(isect.l1->p1)) {isect.l1->p1 = isect.p; ++cut;}
						if (w2.containsPoint(isect.l1->p2)) {isect.l1->p2 = isect.p; ++cut;}

						// if one of the line-ends p1/p2 from wall2 ends within wall1, crop it by setting it to the intersection
						if (w1.containsPoint(isect.l2->p1)) {isect.l2->p1 = isect.p; ++cut;}
						if (w1.containsPoint(isect.l2->p2)) {isect.l2->p2 = isect.p; ++cut;}

					}

					// 2 lines should have been cut. if not, potential issue!
					if (cut != 2) {
						w1.error = true;
						w2.error = true;
					}

				}

			}

			return walls;

		}


		/** if two walls share one node, cut both ends in the middle (like 45 degree) */
		std::vector<Wall> cutConnected(std::vector<Wall> walls) {

			for (size_t i = 0; i < walls.size(); ++i) {
				Wall& w1 = walls[i];

				for (size_t j = i+1; j < walls.size(); ++j) {
					Wall& w2 = walls[j];

					if (i == j) {continue;}

					// if the two walls are note directly connected -> ignore
					if (!w1.directlyConnectedTo(w2)) {continue;}

					const float d = 0.001;
					if			(w1.line->to.eq(w2.line->from, d)) {
						cutInMiddle(w1.getP1(), w1.getP2(),		w2.getP1(), w2.getP2());
						cutInMiddle(w1.getP4(), w1.getP3(),		w2.getP4(), w2.getP3());
					} else if	(w1.line->to.eq(w2.line->to, d)) {
						cutInMiddle(w1.getP1(), w1.getP2(),		w2.getP3(), w2.getP4());
						cutInMiddle(w1.getP4(), w1.getP3(),		w2.getP2(), w2.getP1());
					} else if	(w1.line->from.eq(w2.line->to, d)) {
						cutInMiddle(w1.getP3(), w1.getP4(),		w2.getP3(), w2.getP4());
						cutInMiddle(w1.getP2(), w1.getP1(),		w2.getP2(), w2.getP1());
					} else if	(w1.line->from.eq(w2.line->from, d)) {
						cutInMiddle(w1.getP3(), w1.getP4(),		w2.getP1(), w2.getP2());
						cutInMiddle(w1.getP2(), w1.getP1(),		w2.getP4(), w2.getP3());
					}

				}

			}

			return walls;

		}

	};

}

#endif // FLOORPLAN_3D_WALLSVIACUTTEDQUADS_H