#ifdef WITH_TESTS

#include <fstream>

#include "../Tests.h"

#include "../../grid/factory/v2/GridFactory.h"
#include "../../grid/factory/v2/GridNodeImportance.h"

#include "../../grid/walk/v2/GridWalker.h"
#include "../../grid/walk/v2/modules/WalkModuleHeading.h"
#include "../../grid/walk/v2/modules/WalkModuleHeadingControl.h"
#include "../../grid/walk/v2/modules/WalkModuleFollowDestination.h"

#include "../../sensors/radio/WiFiGridNode.h"


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

#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>


// ENSURE UNIQUE CLASS NAME
struct MyNode1239 : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<10> {
	MyNode1239() {;}
	MyNode1239(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
	static void staticDeserialize(std::istream& inp) {
		WiFiGridNode::staticDeserialize(inp);
	}

	static void staticSerialize(std::ostream& out) {
		WiFiGridNode::staticSerialize(out);
	}
};

// ENSURE UNIQUE CLASS NAME
struct MyState23452 : public WalkState, public WalkStateHeading {
	MyState23452(const GridPoint& pos, const Heading head) : WalkState(pos), WalkStateHeading(head, 0) {;}
};


TEST(GridWalk2, LIVE_error) {


	Grid<MyNode1239> grid(20);
	GridFactory<MyNode1239> gf(grid);

	Floorplan::Floor floor;
	Floorplan::FloorOutlinePolygon poly;
	poly.poly.points.push_back(Point2( 0, 0));
	poly.poly.points.push_back(Point2( 0,50));
	poly.poly.points.push_back(Point2(50,50));
	poly.poly.points.push_back(Point2(50, 0));
	floor.outline.push_back(&poly);
	gf.addFloor(&floor);


	struct Control {
		float turnSinceLastTransition_rad = 0;		// keep the angle as-is!
	} ctrl;

	GridWalker<MyNode1239, MyState23452> walker;
	WalkModuleHeadingControl<MyNode1239, MyState23452, Control> modHead(&ctrl, 3.0f);

	walker.addModule(&modHead);


	MyState23452 state(GridPoint(800,800,0), Heading(3.1415/2));

	K::Gnuplot gp;
	gp << "set xrange[0:5000]\n";
	gp << "set yrange[0:5000]\n";

	K::GnuplotPlot plot;
	K::GnuplotPlotElementLines lines; plot.add(&lines);

	for (int j = 0; j < 100; ++j) {

		state = walker.getDestination(grid, state, 0.4);
		gp << "set label 1 at screen 0.5,0.5 '" << state.heading.direction.getRAD() << "'\n";

		lines.add(K::GnuplotPoint2(state.position.x_cm, state.position.y_cm));

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

	}







}


TEST(GgridWalk2, LIVE_walkHeading) {

	Grid<MyNode1239> grid(20);
	std::ifstream inp("/tmp/grid.dat");
	grid.read(inp);

//	Floorplan::FloorOutlinePolygon poly;
//	poly.method =Floorplan::OutlineMethod::ADD;
//	poly.poly.points.push_back(Point2(0,0));
//	poly.poly.points.push_back(Point2(0,20));
//	poly.poly.points.push_back(Point2(20,20));
//	poly.poly.points.push_back(Point2(20,0));

//	Floorplan::Floor floor;
//	floor.outline.push_back(&poly);


//	GridFactory<MyNode> fac(grid);
//	fac.addFloor(&floor);

	GridWalker<MyNode1239, MyState23452> walker;
	WalkModuleHeading<MyNode1239, MyState23452> wmHead;
	WalkModuleFollowDestination<MyNode1239, MyState23452> wmDest(grid, grid.getNodeFor(GridPoint(7200,4800,400+340+340)));
	//walker.addModule(&wmHead);
	walker.addModule(&wmDest);

	std::vector<GridPoint> points;
	for (int i = 0; i < 100; ++i) {points.push_back(GridPoint(200,200,0));}

	K::Gnuplot gp;
	K::GnuplotSplot splot;
	K::GnuplotSplotElementPoints nodes;	nodes.setPointSize(0.1); nodes.getColor().setHexStr("#888888"); splot.add(&nodes);
	K::GnuplotSplotElementPoints states; states.setPointSize(0.5); states.getColor().setHexStr("#0000ff"); splot.add(&states);

	for (const MyNode1239& n : grid) {
		static int cnt = 0;
		if (++cnt % 5 == 0) {
			nodes.add(K::GnuplotPoint3(n.x_cm, n.y_cm, n.z_cm));
		}
	}


	for (int j = 0; j < 500; ++j) {
		states.clear();
		for (int i = 0; i < points.size(); ++i) {
			MyState23452 start(points[i], Heading(0.0));
			MyState23452 next = walker.getDestination(grid, start, 2.0);
			points[i] = next.position;
			states.add(K::GnuplotPoint3(points[i].x_cm, points[i].y_cm, points[i].z_cm));
		}

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

	}

	int i = 0; (void) i;


}

#endif