/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/opengl/CAngularObservationMesh.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPolyhedron.h>

#include <iostream>

using namespace std;
using namespace mrpt;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using mrpt::opengl::CAngularObservationMesh;
using namespace mrpt::system;

const double GRID_R = 1.0;
const double GRID_G = 1.0;
const double GRID_B = 1.0;

// Comment to increase randomness of poses
#define PAIRED_RANDOM_POSES

#define USE_THREADS

inline double MYRAND1(size_t prec = 64)
{
	return static_cast<double>(rand() % prec) / static_cast<double>(prec - 1);
}

void randomColor(const CRenderizable::Ptr& obj, double alpha)
{
	obj->setColor(MYRAND1(), MYRAND1(), MYRAND1(), alpha);
}

#ifdef USE_THREADS
class PIThreadParam
{
   public:
	const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>* polys{nullptr};
	vector<TSegment3D> intersection;
	PIThreadParam(const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>& p)
		: polys(&p), intersection()
	{
	}
	PIThreadParam() : intersection() {}
	inline static PIThreadParam createObject(
		const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>& p)
	{
		return PIThreadParam(p);
	}
};

void piThreadFunction(PIThreadParam& p)
{
	vector<TObject3D> ints;
	CPolyhedron::getIntersection(p.polys->first, p.polys->second, ints);
	for (const auto& o : ints)
		if (o.isSegment()) p.intersection.emplace_back(o.getAs<TSegment3D>());
}

inline std::thread piCreateThread(PIThreadParam& p)
{
	return std::thread(&piThreadFunction, std::ref(p));
}

class AggregatorFunctor
{
   public:
	vector<TSegment3D>& sgms;
	AggregatorFunctor(vector<TSegment3D>& s) : sgms(s) {}
	inline void operator()(const PIThreadParam& p)
	{
		sgms.insert(sgms.end(), p.intersection.begin(), p.intersection.end());
	}
};
#endif

CSetOfLines::Ptr getIntersections(
	const vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>>& v)
{
	vector<TSegment3D> sgms;
#ifdef USE_THREADS
	vector<PIThreadParam> pars(v.size());
	vector<std::thread> threads(v.size());
	transform(v.begin(), v.end(), pars.begin(), &PIThreadParam::createObject);
	transform(pars.begin(), pars.end(), threads.begin(), &piCreateThread);
	for_each(threads.begin(), threads.end(), [](std::thread& t) { t.join(); });
	for_each(pars.begin(), pars.end(), AggregatorFunctor(sgms));
#else
	vector<TObject3D> ints, TMP;
	for (vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>>::const_iterator it =
			 v.begin();
		 it != v.end(); ++it)
	{
		CPolyhedron::getIntersection(it->first, it->second, TMP);
		ints.insert(ints.end(), TMP.begin(), TMP.end());
	}
	TObject3D::getSegments(ints, sgms);
#endif
	CSetOfLines::Ptr lns = CSetOfLines::Create(sgms);
	lns->setLineWidth(9);
	randomColor(lns, 1.0);
	return lns;
}

inline double randomAngle(size_t prec = 64) { return MYRAND1(prec) * M_PI; }
inline double randomZ(double space = 25, size_t prec = 64)
{
	return space * (MYRAND1(prec) - 0.5);
}

pair<CPolyhedron::Ptr, CPolyhedron::Ptr> addPairOfPolys(
	CPolyhedron::Ptr p1, CPolyhedron::Ptr p2, CSetOfObjects::Ptr& objs,
	double x, double y)
{
	p1->makeConvexPolygons();
	p2->makeConvexPolygons();
#ifdef PAIRED_RANDOM_POSES
	CPose3D pose =
		CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
	p1->setPose(pose);
	p2->setPose(pose);
#else
	CPose3D pose1 =
		CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
	CPose3D pose2 =
		CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
	p1->setPose(pose1);
	p2->setPose(pose2);
#endif
	randomColor(p1, 0.5);
	randomColor(p2, 0.5);
	objs->insert(p1);
	objs->insert(p2);
	return make_pair(p1, p2);
}

void display()
{
	CDisplayWindow3D window("Polyhedra Intersection demo", 640, 480);
	window.resize(640, 480);
	COpenGLScene::Ptr scene1 = COpenGLScene::Create();
	opengl::CGridPlaneXY::Ptr plane1 =
		CGridPlaneXY::Create(-25, 25, -25, 25, 0, 1);
	plane1->setColor(GRID_R, GRID_G, GRID_B);
	scene1->insert(plane1);
	scene1->insert(CAxis::Create(-5, -5, -5, 5, 5, 5, 2.5, 3, true));
	CSetOfObjects::Ptr objs = CSetOfObjects::Create();
	vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>> polys;
	polys.reserve(16);
	// Addition of polyhedra. Add more polyhedra at wish, but try to avoid
	// intersections with other pairs, for better visualization.
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateHexahedron(10), CPolyhedron::CreateOctahedron(10),
		objs, -12.5, -12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateIcosahedron(10), CPolyhedron::CreateDodecahedron(10),
		objs, -12.5, 12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateRhombicuboctahedron(10),
		CPolyhedron::CreateCuboctahedron(10), objs, 12.5, 12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateArchimedeanRegularAntiprism(4, 9),
		CPolyhedron::CreateRegularDoublePyramid(9, 10, 15, 6), objs, 12.5,
		-12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateCuboctahedron(10),
		CPolyhedron::CreateRhombicDodecahedron(10), objs, -37.5, -37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateRhombicuboctahedron(10),
		CPolyhedron::CreateDeltoidalIcositetrahedron(10), objs, -37.5, -12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateIcosidodecahedron(10),
		CPolyhedron::CreateRhombicTriacontahedron(10), objs, -37.5, 12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateRhombicosidodecahedron(10),
		CPolyhedron::CreateDeltoidalHexecontahedron(10), objs, -37.5, 37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTruncatedTetrahedron(10),
		CPolyhedron::CreateTriakisTetrahedron(10), objs, -12.5, -37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTruncatedHexahedron(10),
		CPolyhedron::CreateTriakisOctahedron(10), objs, -12.5, 37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTruncatedOctahedron(10),
		CPolyhedron::CreateTetrakisHexahedron(10), objs, 12.5, -37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTruncatedDodecahedron(10),
		CPolyhedron::CreateTriakisIcosahedron(10), objs, 12.5, 37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTruncatedIcosahedron(10),
		CPolyhedron::CreatePentakisDodecahedron(10), objs, 37.5, -37.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateRandomPolyhedron(10),
		CPolyhedron::CreateRandomPolyhedron(10), objs, 37.5, -12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateDodecahedron(10),
		CPolyhedron::CreateDeltoidalHexecontahedron(10), objs, 37.5, 12.5));
	polys.push_back(addPairOfPolys(
		CPolyhedron::CreateTriakisIcosahedron(10),
		CPolyhedron::CreatePentakisDodecahedron(10), objs, 37.5, 37.5));
	objs << getIntersections(polys);

	scene1->insert(objs);
	window.get3DSceneAndLock() = scene1;
	window.unlockAccess3DScene();
	window.setCameraElevationDeg(25.0f);
	window.forceRepaint();
	window.waitForKey();
}

int main()
{
	srand((unsigned int)mrpt::system::extractDayTimeFromTimestamp(
		mrpt::system::now()));
	try
	{
		display();
		return 0;
	}
	catch (exception& e)
	{
		cout << "Error: " << e.what() << '.' << endl;
		return -1;
	}
	catch (...)
	{
		cout << "Unknown Error.\n";
		return -1;
	}
}
