/* +------------------------------------------------------------------------+
   |                     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/containers/traits_map.h>
#include <mrpt/graphs/CNetworkOfPoses.h>
#include <mrpt/graphs/dijkstra.h>
#include <mrpt/random.h>
#include <mrpt/system/CTimeLogger.h>

#include "common.h"

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::graphs;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace std;

// ------------------------------------------------------
//				Benchmark: Graphs
// ------------------------------------------------------

template <class EDGE_TYPE, class MAPIMPL>
double graphs_test_populate(int nEdges, int _N)
{
	const long N = _N;

	std::vector<mrpt::graphs::CNetworkOfPoses<EDGE_TYPE, MAPIMPL>> gs(N);

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		mrpt::graphs::CNetworkOfPoses<EDGE_TYPE, MAPIMPL>& g = gs[i];
		for (int j = 0; j < nEdges; ++j)
		{
			g.insertEdge(j, j + 1, EDGE_TYPE());
		}
	}
	return tictac.Tac() / N;
}

template <class EDGE_TYPE, class MAPIMPL>
double graphs_test_populate_at_end(int nEdges, int _N)
{
	const long N = _N;

	std::vector<mrpt::graphs::CNetworkOfPoses<EDGE_TYPE, MAPIMPL>> gs(N);

	CTicTac tictac;
	for (long i = 0; i < N; i++)
	{
		mrpt::graphs::CNetworkOfPoses<EDGE_TYPE, MAPIMPL>& g = gs[i];
		for (int j = 0; j < nEdges; ++j)
		{
			// g.insertEdgeAtEnd(j,j+1, EDGE_TYPE() );
			g.edges.insert(
				g.edges.end(),
				std::make_pair(std::make_pair(j, j + 1), EDGE_TYPE()));
		}
	}
	return tictac.Tac() / N;
}

template <class EDGE_TYPE, class MAPS_IMPLEMENTATION>
double graphs_dijkstra(int nNodes, int _N)
{
	const long N = _N;

	getRandomGenerator().randomize(111);
	// Generate random graph:
	using graph_t =
		mrpt::graphs::CNetworkOfPoses<EDGE_TYPE, MAPS_IMPLEMENTATION>;
	graph_t gs;
	{
		double edge_node_ratio = 2.0;
		for (unsigned int i = 0; i < (unsigned int)nNodes; i++)
		{
			size_t nEdges = 1 +
				(mrpt::random::getRandomGenerator().drawUniform32bit() %
				 int(edge_node_ratio - 1));
			for (TNodeID k = 0; k < nEdges; k++)
			{
				TNodeID dest;
				dest = i + 1;
				if (k > 0)
				{
					while (dest == i && dest != i + 1)	// Avoid self-loops!
						dest = mrpt::random::getRandomGenerator()
								   .drawUniform32bit() %
							nNodes;
				}
				gs.insertEdge(i, dest, EDGE_TYPE());
			}
		}
	}

	CTimeLogger tims;
	for (long i = 0; i < N; i++)
	{
		tims.enter("op");
		mrpt::graphs::CDijkstra<graph_t> dij(gs, TNodeID(0));
		tims.leave("op");
		// Don't count the time of the destructor.
	}
	tims.enable(false);
	double ret = tims.getMeanTime("op");
	tims.clear(true /* deep clear */);
	return ret;
}

// ------------------------------------------------------
// register_tests_graph
// ------------------------------------------------------
void register_tests_graph()
{
	using namespace mrpt::containers;

	getRandomGenerator().randomize(1234);

	lstTests.emplace_back(
		"graph(2d): insertEdge x 1e3",
		graphs_test_populate<CPose2D, map_traits_stdmap>, 1e3, 1000);
	lstTests.emplace_back(
		"graph(2d,vec): insertEdge x 1e3",
		graphs_test_populate<CPose2D, map_traits_map_as_vector>, 1e3, 1000);

	lstTests.emplace_back(
		"graph(2d): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPose2D, map_traits_stdmap>, 1e3, 1000);
	lstTests.emplace_back(
		"graph(2d,vec): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPose2D, map_traits_map_as_vector>, 1e3,
		1000);

	lstTests.emplace_back(
		"graph(2d pdf): insertEdge x 1e3",
		graphs_test_populate<CPosePDFGaussianInf, map_traits_stdmap>, 1e3,
		1000);
	lstTests.emplace_back(
		"graph(2d pdf,vec): insertEdge x 1e3",
		graphs_test_populate<CPosePDFGaussianInf, map_traits_map_as_vector>,
		1e3, 1000);

	lstTests.emplace_back(
		"graph(2d pdf): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPosePDFGaussianInf, map_traits_stdmap>,
		1e3, 1000);
	lstTests.emplace_back(
		"graph(2d pdf,vec): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<
			CPosePDFGaussianInf, map_traits_map_as_vector>,
		1e3, 1000);

	lstTests.emplace_back(
		"graph(2d): insertEdge x 1e4",
		graphs_test_populate<CPose2D, map_traits_stdmap>, 1e4, 250);
	lstTests.emplace_back(
		"graph(2d,vec): insertEdge x 1e4",
		graphs_test_populate<CPose2D, map_traits_map_as_vector>, 1e4, 250);

	lstTests.emplace_back(
		"graph(2d): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPose2D, map_traits_stdmap>, 1e4, 250);
	lstTests.emplace_back(
		"graph(2d,vec): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPose2D, map_traits_map_as_vector>, 1e4,
		250);

	lstTests.emplace_back(
		"graph(2d pdf): insertEdge x 1e4",
		graphs_test_populate<CPosePDFGaussianInf, map_traits_stdmap>, 1e4, 250);
	lstTests.emplace_back(
		"graph(2d pdf,vec): insertEdge x 1e4",
		graphs_test_populate<CPosePDFGaussianInf, map_traits_map_as_vector>,
		1e4, 250);

	lstTests.emplace_back(
		"graph(2d pdf): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPosePDFGaussianInf, map_traits_stdmap>,
		1e4, 250);
	lstTests.emplace_back(
		"graph(2d pdf,vec): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<
			CPosePDFGaussianInf, map_traits_map_as_vector>,
		1e4, 250);

	lstTests.emplace_back(
		"graph(3d): insertEdge x 1e3",
		graphs_test_populate<CPose3D, map_traits_stdmap>, 1e3, 1000);
	lstTests.emplace_back(
		"graph(3d,vec): insertEdge x 1e3",
		graphs_test_populate<CPose3D, map_traits_map_as_vector>, 1e3, 1000);

	lstTests.emplace_back(
		"graph(3d): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPose3D, map_traits_stdmap>, 1e3, 1000);
	lstTests.emplace_back(
		"graph(3d,vec): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPose3D, map_traits_map_as_vector>, 1e3,
		1000);

	lstTests.emplace_back(
		"graph(3d pdf): insertEdge x 1e3",
		graphs_test_populate<CPose3DPDFGaussianInf, map_traits_stdmap>, 1e3,
		1000);
	lstTests.emplace_back(
		"graph(3d pdf,vec): insertEdge x 1e3",
		graphs_test_populate<CPose3DPDFGaussianInf, map_traits_map_as_vector>,
		1e3, 1000);

	lstTests.emplace_back(
		"graph(3d pdf): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<CPose3DPDFGaussianInf, map_traits_stdmap>,
		1e3, 1000);
	lstTests.emplace_back(
		"graph(3d pdf,vec): insertEdgeAtEnd x 1e3",
		graphs_test_populate_at_end<
			CPose3DPDFGaussianInf, map_traits_map_as_vector>,
		1e3, 1000);

	lstTests.emplace_back(
		"graph(3d): insertEdge x 1e4",
		graphs_test_populate<CPose3D, map_traits_stdmap>, 1e4, 250);
	lstTests.emplace_back(
		"graph(3d,vec): insertEdge x 1e4",
		graphs_test_populate<CPose3D, map_traits_map_as_vector>, 1e4, 250);

	lstTests.emplace_back(
		"graph(3d): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPose3D, map_traits_stdmap>, 1e4, 250);
	lstTests.emplace_back(
		"graph(3d,vec): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPose3D, map_traits_map_as_vector>, 1e4,
		250);

	lstTests.emplace_back(
		"graph(3d pdf): insertEdge x 1e4",
		graphs_test_populate<CPose3DPDFGaussianInf, map_traits_stdmap>, 1e4,
		250);
	lstTests.emplace_back(
		"graph(3d pdf,vec): insertEdge x 1e4",
		graphs_test_populate<CPose3DPDFGaussianInf, map_traits_map_as_vector>,
		1e4, 250);

	lstTests.emplace_back(
		"graph(3d pdf): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<CPose3DPDFGaussianInf, map_traits_stdmap>,
		1e4, 250);
	lstTests.emplace_back(
		"graph(3d pdf,vec): insertEdgeAtEnd x 1e4",
		graphs_test_populate_at_end<
			CPose3DPDFGaussianInf, map_traits_map_as_vector>,
		1e4, 250);

	lstTests.emplace_back(
		"graph(3d): dijkstra 1e2 nodes",
		graphs_dijkstra<CPose3D, map_traits_stdmap>, 1e2, 500);
	lstTests.emplace_back(
		"graph(3d,vec): dijkstra 1e2 nodes",
		graphs_dijkstra<CPose3D, map_traits_map_as_vector>, 1e2, 500);

	lstTests.emplace_back(
		"graph(3d): dijkstra 1e3 nodes",
		graphs_dijkstra<CPose3D, map_traits_stdmap>, 1e3, 500);
	lstTests.emplace_back(
		"graph(3d,vec): dijkstra 1e3 nodes",
		graphs_dijkstra<CPose3D, map_traits_map_as_vector>, 1e3, 500);

	lstTests.emplace_back(
		"graph(3d): dijkstra 1e4 nodes",
		graphs_dijkstra<CPose3D, map_traits_stdmap>, 1e4, 50);
	lstTests.emplace_back(
		"graph(3d,vec): dijkstra 1e4 nodes",
		graphs_dijkstra<CPose3D, map_traits_map_as_vector>, 1e4, 50);

	lstTests.emplace_back(
		"graph(3d): dijkstra 1e5 nodes",
		graphs_dijkstra<CPose3D, map_traits_stdmap>, 1e5, 50);
	lstTests.emplace_back(
		"graph(3d,vec): dijkstra 1e5 nodes",
		graphs_dijkstra<CPose3D, map_traits_map_as_vector>, 1e5, 50);

	lstTests.emplace_back(
		"graph(2d): dijkstra 1e5 nodes",
		graphs_dijkstra<CPose2D, map_traits_stdmap>, 1e5, 50);
	lstTests.emplace_back(
		"graph(2d,vec): dijkstra 1e5 nodes",
		graphs_dijkstra<CPose2D, map_traits_map_as_vector>, 1e5, 50);
}
