#ifndef FLOORPLANHELPER2_H
#define FLOORPLANHELPER2_H

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

#include "Floorplan.h"

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

public:

	/** 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