#ifdef WITH_TESTS

#include "../Tests.h"

#include "../../navMesh/NavMeshFactory.h"
#include "../../navMesh/walk/NavMeshSub.h"
#include "../../navMesh/meta/NavMeshDijkstra.h"
#include "../../navMesh/NavMeshDebug.h"

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


using namespace NM;

struct MyNMT1231902345 : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {

	MyNMT1231902345(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : NavMeshTriangle(p1, p2, p3, type) {;}

};

TEST(NavMeshDijkstra, build) {

	NavMeshSettings set;
	NavMesh<MyNMT1231902345> nm;



	Floorplan::IndoorMap map;
	Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
	Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);

	// circle (many triangles)
	int i = 0;
	for (float f = 0; f < M_PI*2; f += 0.8) {
		const float x = std::cos(f) * 10;
		const float y = std::sin(f) * 10;
		outline.poly.points.push_back(Point2(x,y));
		++i;
	}

	outline.outdoor = false;
	outline.method = Floorplan::OutlineMethod::ADD;

	Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
	remove.outdoor = false;
	remove.method = Floorplan::OutlineMethod::REMOVE;
	remove.poly.points.push_back(Point2(+4,-3));
	remove.poly.points.push_back(Point2(+11,-3));
	remove.poly.points.push_back(Point2(+11,+1));
	remove.poly.points.push_back(Point2(+4,+1));

	Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2);
	remove2.outdoor = false;
	remove2.method = Floorplan::OutlineMethod::REMOVE;
	remove2.poly.points.push_back(Point2(-11,-2));
	remove2.poly.points.push_back(Point2(-2,-2));
	remove2.poly.points.push_back(Point2(-2,+2));
	remove2.poly.points.push_back(Point2(-11,+2));

	NavMeshFactory<MyNMT1231902345> fac(&nm, set);
	fac.build(&map);

//	nm.add(Point3(0,0,0), Point3(10,10,0), Point3(0,10,0), 1);
//	nm.add(Point3(0,10,0), Point3(10,10,0), Point3(0,20,0), 1);
//	nm.add(Point3(0,20,0), Point3(10,10,0), Point3(10,20,0), 1);


	NavMeshDebug dbg;
	dbg.addMesh(nm);
	dbg.draw();

	NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0));

	dbg.addDijkstra(nm);
	dbg.draw();

	int xxx = 0; (void) xxx;

}


TEST(NavMeshDijkstra, build2) {

	NavMeshSettings set;
	NavMesh<MyNMT1231902345> nm;

	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");
	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/SHL41_nm.xml");
	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/museum31.xml");



	NavMeshFactory<MyNMT1231902345> fac(&nm, set);
	fac.build(map);

	NavMeshDebug dbg;
	dbg.addMesh(nm);
	dbg.draw();

	NM::NavMeshDijkstra::stamp(nm, Point3(4,4,0));

	dbg.addDijkstra(nm);
	dbg.draw();

	int xxx = 0; (void) xxx;

}

TEST(NavMeshDijkstra, path) {

	NavMeshSettings set;
	NavMesh<MyNMT1231902345> nm;

//	Floorplan::IndoorMap map;
//	Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3;
//	Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline);

//	// circle (many triangles)
//	int i = 0;
//	for (float f = 0; f < M_PI*2; f += 0.8) {
//		const float x = std::cos(f) * 10;
//		const float y = std::sin(f) * 10;
//		outline.poly.points.push_back(Point2(x,y));
//		++i;
//	}

//	outline.outdoor = false;
//	outline.method = Floorplan::OutlineMethod::ADD;

//	Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove);
//	remove.outdoor = false;
//	remove.method = Floorplan::OutlineMethod::REMOVE;
//	remove.poly.points.push_back(Point2(+4,-3));
//	remove.poly.points.push_back(Point2(+11,-3));
//	remove.poly.points.push_back(Point2(+11,+1));
//	remove.poly.points.push_back(Point2(+4,+1));

//	Floorplan::FloorOutlinePolygon remove2; floor.outline.push_back(&remove2);
//	remove2.outdoor = false;
//	remove2.method = Floorplan::OutlineMethod::REMOVE;
//	remove2.poly.points.push_back(Point2(-11,-2));
//	remove2.poly.points.push_back(Point2(-2,-2));
//	remove2.poly.points.push_back(Point2(-2,+2));
//	remove2.poly.points.push_back(Point2(-11,+2));

	//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/mnt/vm/paper/diss/data/maps/map_stair1.xml");
	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/paper/diss/data/maps/map_stair1.xml");


	NavMeshFactory<MyNMT1231902345> fac(&nm, set);
	fac.build(map);


	NavMeshDebug dbg;
	dbg.addMesh(nm);
	dbg.draw();

	NM::NavMeshRandom<MyNMT1231902345> rnd = nm.getRandom();
	const Point3 dst = rnd.draw().pos;

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


		NM::NavMeshLocation<MyNMT1231902345> start = rnd.draw();
		start.tria->getDistanceToDestination<MyNMT1231902345>(start.pos);	// just a compiler-test

		NM::NavMeshDijkstra::stamp(nm, dst);

		//NM::NavMeshLocation<MyNMT1231902345> start = start;//nm.getLocation(Point3(0,-6,0));
		std::vector<NM::NavMeshLocation<MyNMT1231902345>> path = start.tria->getPathToDestination<MyNMT1231902345>(start.pos);
		std::cout << path.size() << std::endl;

		dbg.addDijkstra(nm);
		dbg.addDijkstra(path);
		dbg.draw();

		sleep(1);

	}

}

#endif