#ifndef HELPER_H
#define HELPER_H

#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>

#include <Indoor/floorplan/FloorplanFactorySVG.h>

#include "Settings.h"
#include "MyGridNode.h"
#include "OldGroundTruth.h"

class Helper {

private:



public:

	/** convert height (in cm) to floor-numbers */
	static int getFloorNr(float z_cm) {
//		if (z_cm < 360) {return 0;}
//		if (z_cm < 360+340) {return 1;}
//		if (z_cm < 360+340+340) {return 2;}
//		return 3;
		if (z_cm < 380) {return 0;}
		if (z_cm < 380+340) {return 1;}
		if (z_cm < 380+340+340) {return 2;}
		return 3;
	}

	/** convert height (in cm) to floor-numbers */
	static int getFloorNrFloat(const float z_cm) {
		return z_cm / 350.0f;
	}

	static int getHeight(const int floorNr) {
		switch(floorNr) {
			case 0: return 0;
			case 1: return 380;
			case 2: return 380+340;
			case 3: return 380+340+340;
			default: throw "error";
		}
	}

	/** align the given value onto the grid */
	template <typename T> static int align(Grid<T>& grid, const int val) {
		volatile const float gridSize_cm = grid.getGridSize_cm();
		return std::round(val / gridSize_cm) * gridSize_cm;
	}

	/** all floors within the building */
	struct FHWSFloors {

		Floor f0, f1, f2, f3;

		Stairs s01, s12, s23;

		LengthF h0, h1, h2, h3;

		// all ground-truth points
		std::unordered_map<int, Point3> gtwp;

		FHWSFloors(LengthF h0, LengthF h1, LengthF h2, LengthF h3) : h0(h0), h1(h1), h2(h2), h3(h3) {;}

	};

	/** load the entire floorplan */
	template <typename T> static FHWSFloors getFloors(Grid<T>& grid) {

		FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
		FHWSFloors f(LengthF::cm(align(grid, getHeight(0))), LengthF::cm(align(grid, getHeight(1))), LengthF::cm(align(grid, getHeight(2))), LengthF::cm(align(grid, getHeight(3))));

		f.f0 = fpFac.getFloor("floor_0");
		f.f1 = fpFac.getFloor("floor_1");
		f.f2 = fpFac.getFloor("floor_2");
		f.f3 = fpFac.getFloor("floor_3");

		f.s01 = fpFac.getStairs("staircase_0_1");
		f.s12 = fpFac.getStairs("staircase_1_2");
		f.s23 = fpFac.getStairs("staircase_2_3");

		OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222);
		OldGroundTruth gtwp05(MiscSettings::floorplan, "ground_truth_0_5", 2.822222);
		OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222);
		OldGroundTruth gtwp15(MiscSettings::floorplan, "ground_truth_1_5", 2.822222);
		OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222);
		OldGroundTruth gtwp25(MiscSettings::floorplan, "ground_truth_2_5", 2.822222);
		OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222);

		for (auto it : gtwp0.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));}
		for (auto it : gtwp1.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));}
		for (auto it : gtwp2.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));}
		for (auto it : gtwp3.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));}

		for (auto it : gtwp05.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(0)+getHeight(1))/2);}
		for (auto it : gtwp15.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(1)+getHeight(2))/2);}
		for (auto it : gtwp25.getWaypoints()) { if (f.gtwp.find(it.first) != f.gtwp.end()) {throw 1;} f.gtwp[it.first] = Point3(it.second.x, it.second.y, (getHeight(2)+getHeight(3))/2);}

		return f;

	}

	template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {

		GridFactory<MyGridNode> gridFac(grid);

		gridFac.addFloor(floors.f0, floors.h0.cm());
		gridFac.addFloor(floors.f1, floors.h1.cm());
		gridFac.addFloor(floors.f2, floors.h2.cm());
		gridFac.addFloor(floors.f3, floors.h3.cm());

		gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
		gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
		gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());

		PlatformStair psUpperLeft;
		psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
		psUpperLeft.s1 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
		psUpperLeft.s2 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
		gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
		gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
		gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
//		vis.gp << "set xrange [1100:1800]\n";
//		vis.gp << "set yrange [4500:5200]\n";

		PlatformStair psUpperRight;
		psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
		psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
		psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
		gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
		gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
		gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
	//	vis.gp << "set xrange [6100:6900]\n";
	//	vis.gp << "set yrange [4500:5200]\n";

		PlatformStair psLowerLeft;
		psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
		psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
		psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
		gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
		gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
		gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
	//	vis.gp << "set xrange [1300:2100]\n";
	//	vis.gp << "set yrange [400:1400]\n";

		// remove all isolated nodes not attached to 300,300,floor0
		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );

		// stamp importance information onto the grid-nodes
		GridImportance gridImp;
		gridImp.addImportance(grid, floors.h0.cm());
		gridImp.addImportance(grid, floors.h1.cm());
		gridImp.addImportance(grid, floors.h2.cm());
		gridImp.addImportance(grid, floors.h3.cm());


	}

};

#endif // HELPER_H