#ifndef FLOORPLANHELPER2_H
#define FLOORPLANHELPER2_H

#include "../../geo/BBox3.h"
#include "../../geo/Line2.h"
#include "../../Assertions.h"

#include "Floorplan.h"

#include "../../sensors/MACAddress.h"

/**
 * helper methods for the floorplan
 */
class FloorplanHelper {

public:


	/** get the AP for the given MAC [if available] */
	static std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> getAP(const Floorplan::IndoorMap* map, const MACAddress& mac) {
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {
				if (MACAddress(ap->mac) == mac) {
					return std::make_pair(ap, f);
				}
			}
		}
		return std::make_pair(nullptr, nullptr);
	}

	/** get all APs within the map */
	static std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> getAPs(const Floorplan::IndoorMap* map) {
		std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> res;
		for (Floorplan::Floor* f : map->floors) {
			for (Floorplan::AccessPoint* ap : f->accesspoints) {
				res.push_back(std::make_pair(ap,f));
			}
		}
		return res;
	}

public:

	/** align all floorplan values to the given grid size. needed for the grid factory */
	static void align(Floorplan::IndoorMap* map, const int gridSize_cm) {
		for (Floorplan::Floor* floor : map->floors) {
			floor->atHeight = align_m(floor->atHeight, gridSize_cm);
			floor->height = align_m(floor->height, gridSize_cm);
			for (Floorplan::Stair* stair : floor->stairs) {
				for (Floorplan::StairPart& part : ((Floorplan::StairFreeform*)stair)->parts) {
					part.start.z = align_m(part.start.z, gridSize_cm);
					part.end.z = align_m(part.end.z, gridSize_cm);
				}
			}
		}
	}

	static inline float align_m(const float val_m, const int gridSize_cm) {
		const float val_cm = val_m * 100;
		const int snapped = std::round(val_cm / gridSize_cm);
		const float res_cm = snapped * gridSize_cm;
		return res_cm / 100.0f;
	}

	/** get a BBox for the whole map */
	static BBox3 getBBox(const Floorplan::IndoorMap* map) {

		BBox3 bbox;

		for (const Floorplan::Floor* floor : map->floors) {
			const BBox3 bbFloor = getBBox(floor);
			bbox.add(bbFloor);
		}

		return bbox;

	}

	/** get a BBox for the whole floor */
	static BBox3 getBBox(const Floorplan::Floor* floor) {

		BBox3 bbox;

		Assert::isNot0(floor->outline.size(), "floor " + floor->name + " has no outline!");

		for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) {
			for (const Point2 p2 : poly->poly.points) {
				bbox.add(Point3(p2.x, p2.y, floor->atHeight));
				bbox.add(Point3(p2.x, p2.y, floor->atHeight + floor->height));
			}
		}

		return bbox;

	}

	/** does the map contain an obstacle that intersects the given line? (in meter!) */
	static bool intersectsObstacle(const Floorplan::IndoorMap* map, const Point3& p1, const Point3& p2) {

		// sanity check
		if (std::abs(p2.z-p1.z) > 0.5) {
			return false;}

		for (const Floorplan::Floor* floor : map->floors) {
			if (intersectsObstacle(floor, p1, p2)) {return true;}
		}

		return false;

	}

	/** does the floor contain an obstacle that intersects the given line? (in meter!) */
	static bool intersectsObstacle(const Floorplan::Floor* floor, const Point3& p1, const Point3& p2) {

		// sanity check
		if (std::abs(p2.z-p1.z) > 0.5) {
			return false;}

		// only inspect elements that lie near the ground
		if (p1.z < floor->getStartingZ() - 0.15)	{return false;}
		if (p1.z > floor->getStartingZ() + 0.15)	{return false;}

		const Line2 line1(p1.x,p1.y,   p2.x,p2.y);

		for (const Floorplan::FloorObstacle* obs : floor->obstacles) {

			if (dynamic_cast<const Floorplan::FloorObstacleLine*>(obs)) {
				const Floorplan::FloorObstacleLine* obsLine = dynamic_cast<const Floorplan::FloorObstacleLine*>(obs);
				const Line2 line2(obsLine->from.x, obsLine->from.y, obsLine->to.x, obsLine->to.y);
				if (line1.getSegmentIntersection(line2)) {return true;}
			} else if (dynamic_cast<const Floorplan::FloorObstacleDoor*>(obs)) {
				;
			} else {
				throw "not yet supported";
			}

		}

		return false;

	}

};

#endif // FLOORPLANHELPER2_H