#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>

#include "Vis.h"
#include "Helper.h"
#include "MyGridNode.h"
#include "Helper.h"
#include "DijkstraMapper.h"

#include "eval/Eval.h"
#include "eval/Eval1.h"
#include "eval/EvalBase.h"

#include "eval/PaperVisImportance.h"
#include "eval/PaperVisDijkstra.h"
#include "eval/PaperVisGrid.h"


float BarometerEvaluation::barometerSigma = NAN;

Settings settings;

void testModelWalk() {

	Grid<MyGridNode> grid(MiscSettings::gridSize_cm);

	Helper::FHWSFloors floors = Helper::getFloors();
	Helper::buildTheGrid(grid, floors);

	MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
	//MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));

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


	vis.gp << "set xrange [1100:1800]\n";
	vis.gp << "set yrange [4500:5200]\n";

//	vis.gp << "set xrange [1000:4000]\n";
//	vis.gp << "set yrange [1000:4000]\n";
//	vis.gp << "set zrange [0:600]\n";

	// switch between different grid-walkers
	GridWalkRandomHeadingUpdate<MyGridNode> walk;
	//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
	//GridWalkPushForward<MyGridNode> walk;
	//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);


	std::vector<GridWalkState<MyGridNode>> states;
	for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }

//	track the number-of-visits for each node to draw something like a particle-heat-map?

	// show the importance factors
	vis.addGrid(grid);
	vis.show();
	sleep(100);
	vis.removeGrid();

	Distribution::Normal<float> wDist(0.3, 0.3);
	Distribution::Normal<float> wHead(0.3, 0.3);

	while(true) {
		for (GridWalkState<MyGridNode>& state : states) {
			state = walk.getDestination(grid, state, std::abs(wDist.draw()), wHead.draw());
		}
		usleep(1000*80);
		vis.showStates(states);
		vis.show();
	}

	sleep(1000);

}



int main(void) {

//	testModelWalk();

	Eval1 eval;
	//eval.path2_forward_simple();
	//eval.path2_forward_path();
	//eval.path3_forward_simple();
	//eval.path3_forward_path();

	//eval.path4_nexus_simple();
	//eval.path4_nexus_imp();
	//eval.path4_nexus_path();
	//eval.path4_nexus_path_b();

	eval.bergwerk_path1_nexus_simple();

	eval.run();

//	PaperVisGrid::showStairs();
//	PaperVisImportance::createImportance();
//	PaperVisImportance::createPath();

	return 0;

}