#ifdef WITH_TESTS

#include <fstream>

#include "../Tests.h"

#include "Plot.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 "../../grid/walk/v2/modules/WalkModuleRelativePressureControl.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>


struct MyNode : public GridPoint, public GridNode {
	MyNode() {;}
	MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};

struct MyState : public WalkState, public WalkStateRelativePressure {
	MyState(const GridPoint& pos, const float hpa) : WalkState(pos), WalkStateRelativePressure(hpa) {;}
};




TEST(GridWalk2RelPressure, LIVE_walkHeading) {

	Grid<MyNode> grid(20);

	int sxy = 300;
	int sz = 600;

	for (int x = 0; x < sxy; x += 40) {
		for (int y = 0; y < sxy; y+= 40) {
			for (int z = 0; z < sz; z += 20) {
				grid.add(MyNode(x,y,z));
			}
		}
	}

	for (int x = 0; x < sxy; x += 40) {
		for (int y = 0; y < sxy; y+= 40) {
			for (int z = 0; z < sz; z += 20) {

				// connect vertically
				for (int dz = -20; dz <= +20; dz += 40) {
					MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
					MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z+dz));
					if (n1&&n2) {
						grid.connectUniDir(*n1, *n2);
					}
				}

				// connect horizontally
				for (int dx = -40; dx <= +40; dx += 40) {
					for (int dy = -40; dy <= +40; dy += 40) {
						if (dx == 0 && dy == 0) {continue;}
						MyNode* n1 = (MyNode*)grid.getNodePtrFor(GridPoint(x,y,z));
						MyNode* n2 = (MyNode*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,z));
						if (n1&&n2) {
							grid.connectUniDir(*n1, *n2);
						}
					}
				}

			}
		}
	}


	struct MyControl {
		struct Barometer {
				float hPaRelativeToT0 = 0.0;
				float estimatedSigma = 0.05;
		} barometer;
	} ctrl;

	GridPoint start(120,120,sz/2);
	const MyNode* n = grid.getNodePtrFor(start);

	MyState ms(start, 0);
	GridWalker<MyNode, MyState> walker;
	WalkModuleRelativePressureControl<MyNode, MyState, MyControl> modPres(&ctrl, 0.1);
	walker.addModule(&modPres);

	Plot p;
	p.addEdges(grid);
	p.addNodes(grid);

	for (int i = 0; i < 1000; ++i) {

		p.clearParticles();

		ctrl.barometer.hPaRelativeToT0 = (std::sin(i/25.0)) * 0.3;
		ms = walker.getDestination(grid, ms, 0.3);
		p.addParticle(ms.startPos.inCentimeter());
		usleep(1000*50);

		p.gp << "set label 97 at screen 0.05, 0.95 ' baro: " << ctrl.barometer.hPaRelativeToT0 << "'\n";
		p.gp << "set label 98 at screen 0.05, 0.90 ' state:" << ms.pressureRelToT0 << "'\n";
		p.gp << "set label 99 at screen 0.05, 0.85 ' diff: " << std::abs(ctrl.barometer.hPaRelativeToT0-ms.pressureRelToT0) << "'\n";

		p.fire();

	}

	p.fire();



	sleep(1000);



}

#endif