#ifdef WITH_TESTS

#include "../Tests.h"

#include "Plot.h"
#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/Importance.h"

#include "../../grid/walk/v2/GridWalker.h"
#include "../../grid/walk/v2/modules/WalkModuleFavorZ.h"
#include "../../grid/walk/v2/modules/WalkModuleSpread.h"
#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
#include "../../grid/walk/v2/modules/WalkModuleNodeImportance.h"
#include "../../grid/walk/v2/modules/WalkModulePreventVisited.h"

#include "../../floorplan/v2/FloorplanReader.h"

#include "../../data/RingBuffer.h"


#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>


// ENSURE UNIQUE CLASS NAME
struct MyNode345092134 : public GridPoint, public GridNode {
	float walkImportance = 0;
	float getWalkImportance() const {return walkImportance;}
	float navImportance = 0;
	float getNavImportance() const {return navImportance;}
	MyNode345092134() {;}
	MyNode345092134(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};

struct MyState102395234 : public WalkState, public WalkStateSpread, public WalkStateFavorZ {
	RingBuffer<int> history;
	MyState102395234(const int x, const int y, const int z) : WalkState(GridPoint(x,y,z)), history(5) {;}
};

//TEST(Walk, plot) {
TEST(Stairs, live_testWalk) {

	Grid<MyNode345092134> g(20);
	GridFactory<MyNode345092134> gf(g);

	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("MapStairs.xml"));
	gf.build(map, nullptr);
	Importance::addImportance(g);

	K::Gnuplot gp;
	K::GnuplotSplot splot;
	K::GnuplotSplotElementPoints pnodes; splot.add(&pnodes);	pnodes.getColor().setHexStr("#888888");	pnodes.setPointType(7);
	K::GnuplotSplotElementPoints pstates; splot.add(&pstates);	pstates.getColor().setHexStr("#0000ff");	pstates.setPointType(7);	pstates.setPointSize(1);

	for (int i = 0; i < g.getNumNodes(); i+=2) {
		const MyNode345092134& gp = g[i];
		pnodes.add(K::GnuplotPoint3(gp.x_cm, gp.y_cm, gp.z_cm));
	}

	std::vector<MyState102395234> states;
	for (int i = 0; i < 5000; ++i) {
		states.push_back(MyState102395234(20,20,0));
	}

	GridWalker<MyNode345092134, MyState102395234> gw;
	WalkModuleFavorZ<MyNode345092134, MyState102395234> modFavorZ;
	WalkModuleSpread<MyNode345092134, MyState102395234> modSpread;
	WalkModuleNodeImportance<MyNode345092134, MyState102395234> modImp;
	//WalkModulePreventVisited<MyNode345092134, MyState102395234> modSkipDup;
	//WalkModuleHeading<MyNode345092134, MyState102395234> modHead;

	gw.addModule(&modImp);
	gw.addModule(&modFavorZ);
	gw.addModule(&modSpread);
	//gw.addModule(&modSkipDup);
	//gw.addModule(&modHead);

	for (int run = 0; run < 5000; ++run) {

		pstates.clear();
		for (int i = 0; i < 100; ++i) {
			MyState102395234& state = states[i];
			state = gw.getDestination(g, state, 0.75);
			pstates.add(K::GnuplotPoint3(state.position.x_cm, state.position.y_cm, state.position.z_cm));
		}

		gp.draw(splot);
		gp.flush();
		usleep(1000*50);

	}






}

#endif