#ifdef WITH_TESTS

#include "../Tests.h"

#include "../../geo/volume/BVH.h"
#include "../../geo/volume/BVHDebug.h"

#include "../../geo/BBox2.h"
#include "../../geo/Line2.h"

#include "../../floorplan/v2/Floorplan.h"
#include "../../floorplan/v2/FloorplanReader.h"
#include "../../wifi/estimate/ray3/ModelFactory.h"

struct WrapperBBox2 {

	static std::vector<Point2> getVertices(const BBox2& bbox) {
		return {bbox.getMin(), bbox.getMax()};
	}

	static std::vector<Point2> getDebugLines(const BBox2& bbox) {

		Point2 p1(bbox.getMin().x, bbox.getMin().y);
		Point2 p2(bbox.getMax().x, bbox.getMin().y);
		Point2 p3(bbox.getMax().x, bbox.getMax().y);
		Point2 p4(bbox.getMin().x, bbox.getMax().y);

		std::vector<Point2> res;
		res.push_back(p1); res.push_back(p2);
		res.push_back(p2); res.push_back(p3);
		res.push_back(p3); res.push_back(p4);
		res.push_back(p4); res.push_back(p1);
		return res;

	}

};

struct WrapperLine2 {

	static std::vector<Point2> getVertices(const Line2& l) {
		return {l.p1, l.p2};
	}

	static std::vector<Point2> getDebugLines(const Line2& l) {
		return {l.p1, l.p2};
	}

};


/*
TEST(Geo_BVH2, tree) {

	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;

	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
	tree.add(bb1);

	BBox3 bb2(Point3(-1,-1,-1), Point3(0,0,0));
	tree.add(bb2);

	tree.optimize();
	tree.show();

	int i = 0; (void) i;

}

TEST(Geo_BVH2, tree2) {

	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;

	BBox3 bb1(Point3(0,0,0), Point3(1,1,1));
	tree.add(bb1);

	BBox3 bb2(Point3(-1,0,0), Point3(0,1,1));
	tree.add(bb2);

	tree.optimize();
	tree.show();

	int i = 0; (void) i;

}

TEST(Geo_BVH2, tree3) {

	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;

	BBox3 bb1 = BBox3::around(Point3(+0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25));
	tree.add(bb1);

	BBox3 bb2 = BBox3::around(Point3(-0.5, +0.5, 0.0), Point3(0.25, 0.25, 0.25));
	tree.add(bb2);

	BBox3 bb3 = BBox3::around(Point3(-0.0, +0.5, 0.0), Point3(0.36, 0.36, 0.36));
	tree.add(bb3);

	BBox3 bb4 = BBox3::around(Point3(-0.0, +0.0, 0.0), Point3(0.5, 0.5, 0.5));
	tree.add(bb4);

	tree.optimize(1);
	tree.show();

	tree.optimize(1);
	tree.show();

	tree.optimize(1);
	tree.show();

	int i = 0; (void) i;

}

TEST(Geo_BVH2, treeMap) {

	std::string file = "/apps/SHL39.xml";
	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(file);

	ModelFactory fac(map);
	fac.setExportCeilings(false);
	fac.setFloors({map->floors[3]});
	std::vector<Obstacle3D> obs = fac.triangulize();

	BVH3Debug<Obstacle3D, BoundingVolumeSphere3, WrapperObs3D> tree;

	for (const Obstacle3D& o : obs) {
		tree.add(o);
	}

	//tree.show(150);

	//int rounds = tree.optimize();

	for (int i = 0; i < 200; ++i) {
		tree.optimize(1);
		//if (i%3==0) {
		tree.show(250, false);
		//}
	}

	int i = 0; (void) i;

}
*/
TEST(Geo_BVH2, treeRandom) {

	//BVH2Debug<Line2, BoundingVolumeCircle2, WrapperLine2> tree;
	BVH2Debug<Line2, BoundingVolumeAABB2, WrapperLine2> tree;

	std::minstd_rand gen;
	std::uniform_real_distribution<float> dPos(-4.0, +4.0);
	std::uniform_real_distribution<float> dDir(+0.1, +0.5);

	for (int i = 0; i < 50; ++i) {
		const Point2 pos(dPos(gen), dPos(gen));
		const Point2 dir(dDir(gen), dDir(gen));
		const Line2 line(pos, pos+dir);
		tree.add(line);
	}

	tree.show();

	if (1 == 0) {
		for (int i = 0; i < 100; ++i) {
			tree.optimize(1);
			tree.show();
			usleep(1000*100);
		}
	}
	int i = 0; (void) i;

}

#endif