#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>

#include "Vis.h"

namespace Settings {
	const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
	const int gridSize_cm = 200;
}

struct MyNode : public GridNode, public GridPoint {
public:
	MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};


int align(const int val) {
	return val / Settings::gridSize_cm * Settings::gridSize_cm;
}

int main(void) {

	Grid<MyNode> grid(Settings::gridSize_cm);
	GridFactory<MyNode> gridFac(grid);


	FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);

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

	Stairs f01 = fpFac.getStairs("staircase_0_1");
	Stairs f12 = fpFac.getStairs("staircase_1_2");
	Stairs f23 = fpFac.getStairs("staircase_2_3");

	const LengthF h0 = LengthF::cm(align(0));
	const LengthF h1 = LengthF::cm(align(360));
	const LengthF h2 = LengthF::cm(align(360+340));
	const LengthF h3 = LengthF::cm(align(360+340+340));

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

	Vis vis;
	vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3);
	vis.addGrid(grid);
	vis.show();

	sleep(1000);

	return 0;

}