#ifndef FLOORPLAN2_H
#define FLOORPLAN2_H

#include <algorithm>
#include <string>
#include <vector>
#include <initializer_list>
#include <unordered_map>

#include "../../geo/Point3.h"
#include "../../geo/Point2.h"
#include "../../geo/EarthPos.h"

namespace Floorplan {

	/** convert string to upper case */
	inline std::string toUpperCase(std::string str) {
		std::transform(str.begin(), str.end(), str.begin(), ::toupper);
		return str;
	}

	/** a free key-value meta element */
	struct Meta {

		struct KeyVal {
			std::string key;
			std::string val;
			KeyVal() {;}
			KeyVal(const std::string& key, const std::string& val) : key(key), val(val) {;}
		};

		const std::string EMPTY = "";

//		std::unordered_map<std::string, std::string> params;
//		const std::string& getVal(const std::string& key) const {
//			const auto it = params.find(key);
//			return (it == params.end()) ? (EMPTY) : (it->second);
//		}

		std::vector<KeyVal> params;

		KeyVal* getKV(const std::string& key) {
			for (KeyVal& kv : params) {
				if (kv.key == key) {return &kv;}
			}
			return nullptr;
		}

		const KeyVal* getKV(const std::string& key) const {
			for (const KeyVal& kv : params) {
				if (kv.key == key) {return &kv;}
			}
			return nullptr;
		}

		const std::string& getVal(const std::string& key) const {
			static const std::string EMPTY = "";
			const KeyVal* kv = getKV(key);
			return (kv) ? (kv->val) : (EMPTY);
		}
		void setVal(const std::string& key, const std::string& val) {(*this)[key] = val;}

		void add(const std::string& key, const std::string& val) {params.push_back(KeyVal(key,val));}

		std::string& operator [] (const std::string& key) {
			KeyVal* kv = getKV(key);
			if (!kv) {params.push_back(KeyVal(key, ""));}
			return getKV(key)->val;
		}

		float getFloat(const std::string& key) const			{ return std::stof(getVal(key)); }
		void setFloat(const std::string& key, const float val)	{ (*this)[key] = std::to_string(val); }

		int getInt(const std::string& key) const				{ return std::stoi(getVal(key)); }
		void setInt(const std::string& key, const int val)		{ (*this)[key] = std::to_string(val); }



		size_t size() const {return params.size();}

		const std::string& getKey(const int idx) const {return params[idx].key;}
		const std::string& getVal(const int idx) const {return params[idx].val;}

		void set(const int idx, const KeyVal& kv) {params[idx] = kv;}
		void setKey(const int idx, const std::string& key) {params[idx].key = key;}
		void setVal(const int idx, const std::string& val) {params[idx].val = val;}

		/** delete the idx-th entry */
		void deleteEntry(const int idx) {params.erase(params.begin()+idx);}

	};

	class HasMeta {

		Meta* meta = nullptr;

	public:

		Meta* getMeta() const {return meta;}

		void setMeta(Meta* meta) {
			if (this->meta) {delete this->meta;}
			this->meta = meta;
		}

	};

	/** 2D polygon */
	struct Polygon2 {
		std::vector<Point2> points;
		Polygon2() : points() {;}
		Polygon2(const std::initializer_list<Point2> lst) : points(lst) {;}
		bool operator == (const Polygon2& o) const {return std::equal(o.points.begin(), o.points.end(), this->points.begin());}
	};

	/** 3D quad */
	struct Quad3 {
		Point3 p1;
		Point3 p2;
		Point3 p3;
		Point3 p4;
		Quad3(const Point3 p1, const Point3 p2, const Point3 p3, const Point3 p4) : p1(p1), p2(p2), p3(p3), p4(p4) {;}
		Quad3 operator * (const float v) const {return Quad3(p1*v, p2*v, p3*v, p4*v);}
		const Point3& operator [] (const int idx) const {
			switch (idx) {
			case 0: return p1;
			case 1: return p2;
			case 2: return p3;
			case 3: return p4;
			default: throw Exception("out of bounds");
			}
		}
		/** same z-value for all points? */
		bool isLeveled() const {
			return (p1.z == p2.z) && (p2.z == p3.z) && (p3.z == p4.z);
		}
	};

	/** additional type-info for obstacles */
	enum class ObstacleType {
		UNKNOWN,
		WALL,
		WINDOW,
		HANDRAIL,
		PILLAR,
		_END,
	};

	/** available door types */
	enum class DoorType {
		UNKNOWN,
		SWING,			// normal
		DOUBLE_SWING,
		SLIDE,			// schiebetuer
		DOUBLE_SLIDE,
		REVOLVING,		// drehtuer
		_END,
	};

	/** available window types */
	enum class WindowType {
		UNKNOWN,
		_END,
	};

	/** all supported material types */
	enum class Material {
		UNKNOWN,
		CONCRETE,
		WOOD,
		DRYWALL,
		GLASS,
		METAL,
		METALLIZED_GLAS,
		_END,
	};

	enum class POIType {
		ROOM,
	};

	/** types of outlines. either add or remove the selected region */
	enum class OutlineMethod {
		ADD,
		REMOVE,
		_END,
	};

	struct Floor;
	struct FloorOutlinePolygon;
	struct FloorObstacle;
	struct AccessPoint;
	struct Beacon;
	struct FingerprintLocation;
	struct FloorRegion;
	struct UnderlayImage;
	struct POI;
	struct Stair;
	struct Elevator;
	struct GroundTruthPoint;
	struct FloorObstacleWallDoor;
	struct FloorObstacleWallWindow;

	struct FloorOutline : public std::vector<FloorOutlinePolygon*> {
		bool enabled = true;
	};
	struct FloorObstacles : public std::vector<FloorObstacle*> {
		bool enabled = true;
	};
	struct FloorAccessPoints : public std::vector<AccessPoint*> {
		bool enabled = true;
	};
	struct FloorBeacons : public std::vector<Beacon*> {
		bool enabled = true;
	};
	struct FloorFingerprintLocations : public std::vector<FingerprintLocation*> {
		bool enabled = true;
	};
	struct FloorRegions : public std::vector<FloorRegion*> {
		bool enabled = true;
	};
	struct FloorUnderlays : public std::vector<UnderlayImage*> {
		bool enabled = true;
	};
	struct FloorPOIs : public std::vector<POI*> {
		bool enabled = true;
	};
	struct FloorStairs : public std::vector<Stair*> {
		bool enabled = true;
	};
	struct FloorElevators : public std::vector<Elevator*> {
		bool enabled = true;
	};
	struct FloorGroundTruthPoints : public std::vector<GroundTruthPoint*> {
		bool enabled = true;
	};

	/** describes one floor within the map, starting at a given height */
	struct Floor {

		bool enabled = true;
		float atHeight;							// the floor's starting height
		float height;							// the floor's total height (from start)
		std::string name;						// the floor's name
		FloorOutline outline;                   // the floor's outline (ground)
		FloorObstacles obstacles;               // all obstacles (wall, door, window, ..) within the floor
		FloorRegions regions;                   // all regions within the floor (rooms, ...)
		FloorAccessPoints accesspoints;
		FloorBeacons beacons;
		FloorFingerprintLocations fpLocations;	// potential fingerprint locations
		FloorUnderlays underlays;				// underlay images (used for map-building)
		FloorPOIs pois;							// POIs within the floor
		FloorStairs stairs;						// all stairs within one floor
		FloorElevators elevators;				// all elevators within one floor
		FloorGroundTruthPoints gtpoints;        // all ground truth points within one floor
		//FloorKeyValue other;					// other, free elements

		Floor() {;}

		Floor(const Floor& o) = delete;
		void operator = (const Floor& o) = delete;

		float getStartingZ() const {return atHeight;}
		float getEndingZ() const {return atHeight + height;}

	};

	/** location for fingerprint measurements */
	struct FingerprintLocation : public HasMeta {
		std::string name;
		Point2 posOnFloor;
		float heightAboveFloor = 0;
		FingerprintLocation() {;}
		FingerprintLocation(const std::string& name, const Point2 posOnFloor, const float heightAboveFloor) : name(name), posOnFloor(posOnFloor), heightAboveFloor(heightAboveFloor) {;}
		Point3 getPosition(const Floor& floor) const {return Point3(posOnFloor.x, posOnFloor.y, floor.atHeight + heightAboveFloor);}
	};

	/** a POI located somewhere on a floor */
	struct POI {
		POIType type;
		std::string name;
		Point2 pos;
		POI() : type(), name(), pos() {;}
		POI(const POIType type, const std::string& name, const Point2& pos) : type(type), name(name), pos(pos) {;}
		bool operator == (const POI& o) const {return (o.type == type) && (o.name == name) && (o.pos == pos);}
	};

		/** a GroundTruthPoint located somewhere on a floor */
		struct GroundTruthPoint {
			int id; //TODO: this value can be changed and isn't set incremental within the indoormap
			Point3 pos;		// TODO: splint into 2D position + float for "heightAboveGround" [waypoints' height is relative to the floor's height!
			GroundTruthPoint() : id(), pos() {;}
			GroundTruthPoint(const int id, const Point3& pos) : id(id), pos(pos) {;}
			const Point3 getPosition(const Floor& f) const {return pos + Point3(0,0,f.atHeight);}
			bool operator == (const GroundTruthPoint& o) const {return (o.id == id) && (o.pos == pos);}
		};

	/** an AccessPoint located somewhere on a floor */
	struct AccessPoint : public HasMeta {
		std::string name;
		std::string mac;
		Point3 pos;		// z is relative to the floor's height
		struct Model {
			float txp = 0;
			float exp = 0;
			float waf = 0;
		} model;
		AccessPoint() : name(), mac(), pos() {;}
		AccessPoint(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(toUpperCase(mac)), pos(pos) {;}
		bool operator == (const AccessPoint& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
		Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);}	// relative to the floor's ground
	};

	/** a beacon located somewhere on a floor */
	struct Beacon {
		std::string name;
		std::string mac;
		std::string major;
		std::string minor;
		std::string uuid;
		Point3 pos;		// z is relative to the floor's height
		struct Model {
			float txp;
			float exp;
			float waf;
		} model;
		Beacon() : name(), mac(), pos() {;}
		Beacon(const std::string& name, const std::string& mac, const Point3& pos) : name(name), mac(mac), pos(pos) {;}
		bool operator == (const Beacon& o) const {return (o.name == name) && (o.mac == mac) && (o.pos == pos);}
				Point3 getPos(const Floor* f) const {return pos + Point3(0,0,f->atHeight);}	// relative to the floor's ground
	};


	/** a polygon denoting a floor's outline. is either added or removed */
	struct FloorOutlinePolygon {
		OutlineMethod method;
		std::string name;
		Polygon2 poly;
		bool outdoor;	// special marker
		FloorOutlinePolygon() : method(OutlineMethod::ADD), name(), poly(), outdoor(false) {;}
		FloorOutlinePolygon(const OutlineMethod method, const std::string& name, const Polygon2& poly, bool outdoor) : method(method), name(name), poly(poly), outdoor(outdoor) {;}
		bool operator == (const FloorOutlinePolygon& o) const {return (o.method == method) && (o.name == name) && (o.poly == poly) && (o.outdoor == outdoor);}
	};


	/** base-class for one obstacle (wall, door, window, pillar, ..) within a floor */
	struct FloorObstacle {
		Material material;
		FloorObstacle() :  material() {;}
		FloorObstacle(const Material material) : material(material) {;}
		virtual ~FloorObstacle() {;}
	};

	/** line obstacle */
	struct FloorObstacleLine : public FloorObstacle {
		ObstacleType type;
		Point2 from;
		Point2 to;
		float thickness_m;
		float height_m = 0;	// 0 = floor's height
		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_m) {;}
		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;}
	};

	/** circle obstacle */
	struct FloorObstacleCircle : public FloorObstacle {
		Point2 center;
		float radius;
		float height = 0;	// 0 = floor's height
		FloorObstacleCircle(const Material material, const Point2 center, const float radius, const float height=0) : FloorObstacle(material), center(center), radius(radius), height(height) {;}
		FloorObstacleCircle(const Material material, const float cx, const float cy, const float radius, const float height=0) : FloorObstacle(material), center(cx,cy), radius(radius), height(height) {;}
		//float getHeight(const Floor* f) const {return (height > 0) ? (height) : (f->height);}
	};

	/** door obstacle */
	struct FloorObstacleDoor : public FloorObstacle {
		DoorType type;
		Point2 from;
		Point2 to;
		float height;
		bool swap;
		FloorObstacleDoor(const DoorType type, const Material material, const Point2 from, const Point2 to) : FloorObstacle(material), type(type), from(from), to(to), height(2.1), swap(false) {;}
		FloorObstacleDoor(const DoorType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float height, const bool swap) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), height(height), swap(swap) {;}
		float getSize() const {return (to-from).length();}
	};


	/** wall obstacle */
	struct FloorObstacleWall: public FloorObstacle {
		ObstacleType type;
		Point2 from;
		Point2 to;
		float thickness_m;
		float height_m = 0;	// 0 = floor's height
		std::vector<FloorObstacleWallDoor*> doors;
		std::vector<FloorObstacleWallWindow*> windows;
		FloorObstacleWall(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_m) {;}
		FloorObstacleWall(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;}
	};

	/** wall->door obstacle */
	struct FloorObstacleWallDoor : public FloorObstacle {
		DoorType type;
		float atLinePos;
		float width;
		float height;
		bool leftRight = false;
		bool inOut = false;
		FloorObstacleWallDoor(const DoorType type, const Material material, const float atLinePos, const float width, const float height, const bool lr = false, const bool io = false) : FloorObstacle(material), type(type), atLinePos(atLinePos), width(width), height(height), leftRight(lr), inOut(io) {;}
		Point2 getStart(const FloorObstacleWall* wall) const {
			const Point2 dir = wall->to - wall->from;
			return wall->from + dir * atLinePos;
		}
		Point2 getEnd(const FloorObstacleWall* wall) const {
			const Point2 dir = wall->to - wall->from;
			return getStart(wall) + dir.normalized() * (leftRight ? -width : +width);
		}
	};

	/** wall->window obstacle */
	struct FloorObstacleWallWindow : public FloorObstacle {
		WindowType type;
		float atLinePos;
		float startsAtHeight;
		float width;
		float height;
		bool leftRight = false;
		bool inOut = false;
		FloorObstacleWallWindow(const WindowType type, const Material material, const float atLinePos, const float startsAtHeight, const float width, const float height, const bool lr = false, const bool io = false) : FloorObstacle(material), type(type), atLinePos(atLinePos), startsAtHeight(startsAtHeight), width(width), height(height), leftRight(lr), inOut(io) {;}
		Point2 getStart(const FloorObstacleWall* wall) const {
			const Point2 dir = wall->to - wall->from;
			const Point2 cen = wall->from + dir * atLinePos;
			return cen - dir.normalized() * width/2;
		}
		Point2 getEnd(const FloorObstacleWall* wall) const {
			const Point2 dir = wall->to - wall->from;
			const Point2 cen = wall->from + dir * atLinePos;
			return cen + dir.normalized() * width/2;
		}
	};


	/** 3D obstacle */
	struct FloorObstacleObject : public FloorObstacle {
		std::string file;
		Point3 pos;
		Point3 rot;
		Point3 scale = Point3(1,1,1);
		FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot, const Point3 scale) : file(file), pos(pos), rot(rot), scale(scale) {;}
	};




	/** one region (e.g. a room) within the floor, described using a polygon */
	struct FloorRegion {
		std::string name;
		Polygon2 poly;
		FloorRegion() : name(), poly() {;}
		FloorRegion(const std::string& name, const Polygon2& poly) : name(name), poly(poly) {;}
	};

	static Point3 xy0(const Point2 p) {
		return Point3(p.x, p.y, 0);
	}

	/** describes a plane as starting point, ending point and fixed width */
	struct StairPart {

		/** z is relative to the floor's height */
		Point3 start;

		/** z is relative to the floor's height */
		Point3 end;

		/** the width for this element */
		float width;

		/** whether to connect this element with the previous one */
		bool connectWithPrev;

		StairPart() {;}

		StairPart(const Point3 start, const Point3 end, const float width) : start(start), end(end), width(width), connectWithPrev(false) {;}

		StairPart(Point3 center, float deg, float length, float width, float height) {
			this->start = Point3(-length/2, 0, -height/2);
			this->end = Point3(+length/2, 0, +height/2);
			this->width = width;
			rotate(deg/180.0*M_PI);
			move(center);
		}

		/** convenience method for array access [0:1] to start and end */
		Point3& operator [] (const int idx) {
			switch (idx) {
			case 0: return start;
			case 1: return end;
			default: throw "error";
			}
		}

		void rotate(const float rad) {
			const float ca = std::cos(rad);
			const float sa = std::sin(rad);
			start = Point3(ca*start.x - sa*start.y, sa*start.x + ca * start.y, start.z);
			end   = Point3(ca*end.x - sa*end.y, sa*end.x + ca * end.y, end.z);;
		}

		void move(const Point3 p) {
			start += p;
			end += p;
		}

	};


	/** snap quad-edges together if their distance is below the given maximum. is used to close minor gaps */
	static inline void snapQuads(std::vector<Floorplan::Quad3>& quads, const float maxDist) {

		for (Floorplan::Quad3& quad1 : quads) {
			for (int i1 = 0; i1 < 4; ++i1) {
				Point3& p1 = (Point3&) quad1[i1];
				for (const Floorplan::Quad3& quad2 : quads) {

					if (&quad1 == &quad2) {continue;}
					for (int i1 = 0; i1 < 4; ++i1) {
						Point3& p2 = (Point3&) quad2[i1];

						if (p1.getDistance(p2) < maxDist) {
							const Point3 p3 = (p1+p2) / 2;
							p1 = p3;
							p2 = p3;
							break;
						}

					}
				}

			}
		}

	}

	/** convert stair-parts to quads. the scaling factor may be used to slightly grow each quad. e.g. needed to ensure that the quads overlap */
	static inline std::vector<Quad3> getQuads(const std::vector<StairPart>& parts, const Floor* floor, const float s = 1.0f) {

		std::vector<Quad3> vec;

		for (const StairPart& part : parts) {

			const float width = part.width;
			const Point3 start = part.start;
			const Point3 end = part.end;
			const Point3 dir = end - start;				// direction vector
			const Point2 dir2(dir.x, dir.y);			// direction without height (just 2D)
			const Point2 perp = dir2.perpendicular();	// perendicular vector
			const Point2 perpN = perp / perp.length();	// normalized perpendicular vector
			const Point3 p1 = start	+ xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
			const Point3 p2 = start	- xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
			const Point3 p3 = end	- xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
			const Point3 p4 = end	+ xy0(perpN * width*s / 2) + Point3(0,0,floor->atHeight);
			const Quad3 q(p1,p2,p3,p4);
			vec.push_back(q);

			// connect
			if (part.connectWithPrev) {
				if (vec.size() >= 2) {
					Quad3& a = vec[vec.size()-2];
					Quad3& b = vec[vec.size()-1];
					Point3 pa = (a.p3 + b.p2)/2; a.p3 = pa; b.p2 = pa;
					Point3 pb = (a.p4 + b.p1)/2; a.p4 = pb; b.p1 = pb;
				}
			}

		}

		snapQuads(vec, 0.05);
		return vec;

	}



	/** base-class for stairs */
	struct Stair : public HasMeta {
		virtual std::vector<StairPart> getParts() const = 0;
	};

	/** 3-part stair, up[left]->platform->up[right] */
	struct StairFreeform : public Stair {
		std::vector<StairPart> parts;
		StairFreeform() {;}
		std::vector<StairPart> getParts() const {return parts;}
	};

	/**
	 * describes an elevator between two floors
	 * so: the height depends on the height of the floor!
	 */
	struct Elevator {

		/** the elevator's center */
		Point2 center;

		/** the elevator's width (in meter) */
		float width;

		/** the elevator's depth (in meter) */
		float depth;

		/** the elevator's rotation (in radians) */
		float rotation;

		/** the elevator's height (from its starting position) */
		float height_m;

		/** get the 4 corner points for the elevator */
		Polygon2 getPoints() const {
			const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center;
			const Point2 p2 = Point2(-width/2, +depth/2).rotated(rotation) + center;
			const Point2 p3 = Point2(-width/2, -depth/2).rotated(rotation) + center;
			const Point2 p4 = Point2(+width/2, -depth/2).rotated(rotation) + center;
			Polygon2 poly;
			poly.points = {p1, p2, p3, p4};
			return poly;
		}

		/** empty ctor */
		Elevator() : center(), width(2), depth(2), rotation(0) {;}

		/** ctor */
		Elevator(const Point2 center) : center(center), width(2), depth(2), rotation(0) {;}

	};



	/** an image file that can be added to the map */
	struct UnderlayImage {
		std::string name;
		std::string filename;
		Point2 anchor;
		float scaleX;
		float scaleY;
	};



	/** one correspondence point: earth <-> map */
	struct EarthPosMapPos {

		EarthPos posOnEarth;

		Point3 posOnMap_m;

		/** empty ctor */
		EarthPosMapPos() : posOnEarth(), posOnMap_m() {;}

		/** ctor */
		EarthPosMapPos(const EarthPos posOnEarth, const Point3 posOnMap_m) : posOnEarth(posOnEarth), posOnMap_m(posOnMap_m) {;}

	};


	/** describe the floorplan's location on earth */
	struct EarthRegistration {

		bool enabled = true;

		/** all available correspondences: earth <-> map */
		std::vector<EarthPosMapPos*> correspondences;

	};

	/** describes the whole indoor map */
	struct IndoorMap {

		float width;
		float depth;

		std::vector<Floor*> floors;

		/** mapping: floorplan <-> earth */
		EarthRegistration earthReg;

		IndoorMap() {;}

		/** no copy */
		IndoorMap(const IndoorMap& o) = delete;

		/** no copy assign */
		void operator = (const IndoorMap& o) = delete;

	};

}

#endif // FLOORPLAN2_H