#ifdef WITH_TESTS

#include "../Tests.h"

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

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

struct Wrapper {

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

	static std::vector<Point3> getDebugLines(const BBox3& bbox) {

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

		Point3 p5(bbox.getMin().x, bbox.getMin().y, bbox.getMax().z);
		Point3 p6(bbox.getMax().x, bbox.getMin().y, bbox.getMax().z);
		Point3 p7(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z);
		Point3 p8(bbox.getMin().x, bbox.getMax().y, bbox.getMax().z);

		std::vector<Point3> 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);

		res.push_back(p5); res.push_back(p6);
		res.push_back(p6); res.push_back(p7);
		res.push_back(p7); res.push_back(p8);
		res.push_back(p8); res.push_back(p5);

		res.push_back(p1); res.push_back(p5);
		res.push_back(p2); res.push_back(p6);
		res.push_back(p3); res.push_back(p7);
		res.push_back(p4); res.push_back(p8);
		return res;

	}

};

struct WrapperObs3D {

	static std::vector<Point3> getVertices(const Obstacle3D& o) {
		std::vector<Point3> pts;
		for (const Triangle3& tria : o.triangles) {
			pts.push_back(tria.p1);
			pts.push_back(tria.p2);
			pts.push_back(tria.p3);
		}
		return pts;
	}

	static std::vector<Point3> getDebugLines(const Obstacle3D& o) {
		std::vector<Point3> pts;
		for (const Triangle3& tria : o.triangles) {
			pts.push_back(tria.p1); pts.push_back(tria.p2);
			pts.push_back(tria.p2); pts.push_back(tria.p3);
			pts.push_back(tria.p3); pts.push_back(tria.p1);
		}
		return pts;
	}

};



TEST(BVH, 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(BVH, 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(BVH, 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(BVH, 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(BVH, treeRandom) {

	BVH3Debug<BBox3, BoundingVolumeSphere3, Wrapper> tree;

	std::minstd_rand gen;
	std::uniform_real_distribution<float> dPos(-4.0, +4.0);
	std::uniform_real_distribution<float> dSize(+0.3, +1.0);

	for (int i = 0; i < 50; ++i) {
		const Point3 pos(dPos(gen), dPos(gen), dPos(gen));
		const Point3 size(dSize(gen), dSize(gen), dSize(gen));
		BBox3 bb = BBox3::around(pos, size);
		tree.add(bb);
	}

	tree.show();

//	for (int i = 0; i < 25; ++i) {
//		tree.optimize(1);
//		tree.show();
//	}

	int i = 0; (void) i;

}

#endif