#ifdef WITH_TESTS

#include <fstream>

#include "../Tests.h"

#include "Plot.h"

#include "../../floorplan/v2/FloorplanReader.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 MyNode12456012 : public GridPoint, public GridNode {
	MyNode12456012() {;}
	MyNode12456012(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
};

struct MyState0012345 : public WalkState, public WalkStateHeading {
	MyState0012345(const GridPoint& pos, const Heading& heading, const float headingError) : WalkState(pos), WalkStateHeading(heading, headingError) {;}
};


TEST(GridWalk2HeadingControl, Heading) {


	const Heading head0(0, 0,   0, +20);
	const Heading head1(0, 0,  20, +20);
	const Heading head2(0, 0,  20,   0);
	const Heading head3(0, 0,  20, -20);
	const Heading head4(0, 0,   0, -20);

	const Heading baseHead = 0;

	const float diff0 = head0.getDiffHalfRAD(baseHead);
	const float diff1 = head1.getDiffHalfRAD(baseHead);
	const float diff2 = head2.getDiffHalfRAD(baseHead);
	const float diff3 = head3.getDiffHalfRAD(baseHead);
	const float diff4 = head4.getDiffHalfRAD(baseHead);

	const float d = 0.0001;

	ASSERT_NEAR(0, diff2, d);
	ASSERT_NEAR(diff1, diff3, d);
	ASSERT_NEAR(diff0, diff4, d);

}

TEST(GridWalk2HeadingControl, LIVE_walkHeading) {

	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(getDataFile("WalkHeadingMap.xml"));

	Grid<MyNode12456012> grid(25);
	GridFactory<MyNode12456012> fac(grid);
	fac.build(map);

	const int sxy = 2000;

//	int sxy = 2000;

//	for (int x = 0; x < sxy; x += 20) {
//		for (int y = 0; y < sxy; y+= 20) {
//			grid.add(MyNode12456012(x,y,0));
//		}
//	}

//	for (int x = 0; x < sxy; x += 20) {
//		for (int y = 0; y < sxy; y+= 20) {

//			MyNode12456012* n1 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x,y,0));

//			// connect horizontally
//			for (int dx = -20; dx <= +20; dx += 20) {
//				for (int dy = -20; dy <= +20; dy += 20) {

//					if (dx == 0 && dy == 0) {continue;}
//					MyNode12456012* n2 = (MyNode12456012*)grid.getNodePtrFor(GridPoint(x+dx,y+dy,0));
//					if (n2) { grid.connectUniDir(*n1, *n2); }

//				}

//			}

//		}
//	}


	struct MyControl {
		float turnAngle = 0;
	} ctrl;


	// one particle
	struct Particle {
		GridPoint pos;
		Heading head;
		float headErr = 0;
		Particle(const GridPoint pos, const Heading head) : pos(pos), head(head) {;}
	};

	GridWalker<MyNode12456012, MyState0012345> walker;
	WalkModuleHeadingControl<MyNode12456012, MyState0012345, MyControl> modPres(&ctrl, 0.0);
	walker.addModule(&modPres);

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

	// noisy step size
	std::minstd_rand gen;
	std::normal_distribution<float> dist(0.75, 0.10);

	for (float rad = 0; rad < 6*M_PI; rad += M_PI*0.125) {

		// setup particles
		std::vector<Particle> particles;
		for (int i = 0; i < 75; ++i) {
			particles.push_back( Particle(GridPoint(sxy/2, sxy/2, 0), 0.0f) );
		}

		// run
		for (int i = 0; i < 30; ++i) {

			ctrl.turnAngle = (i == 0) ? (rad) : (0);

			p.clearParticles();

			for (Particle& particle : particles) {

				// particle -> state
				MyState0012345 state(particle.pos, particle.head, particle.headErr);

				// process state
				const float dist_m = dist(gen);
				state = walker.getDestination(grid, state, dist_m);

				// state -> particle
				particle.head = state.heading.direction;
				particle.pos = state.position;
				particle.headErr = state.heading.error;

				p.addParticle(Point3(particle.pos.x_cm, particle.pos.y_cm, particle.pos.z_cm));

			}

			Point2 p1(sxy/2, sxy/2);
			Point2 p2 = p1 + (Point2(1,0).rotated(rad) * sxy/2);
			p.gp << "set arrow 1 from " << p1.x << "," << p1.y << "," << 0 << " to " << p2.x << "," << p2.y << "," << 0 << " front \n";

			p.fire();
			usleep(1000*33);

		}

	}

}

#endif