/* +------------------------------------------------------------------------+
   |                     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/poses/CPose2D.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DInterpolator.h>
#include <mrpt/random/RandomGenerators.h>
#include <mrpt/system/datetime.h>

#include "common.h"

using mrpt::DEG2RAD;

// ------------------------------------------------------
//				Benchmark Pose Interpolation
// ------------------------------------------------------

// interpolator tests ======================
template <
	typename PATH_T, typename pose_t, bool INSERT_AT_END,
	bool BENCHMARK_INSERT_nQUERY>
double pose_interp_test(int a1, int a2)
{
	const long N = 400000;
	mrpt::system::CTicTac tictac;

	pose_t a =
		pose_t(mrpt::poses::CPose3D(1.0, 2.0, 0, 10.0_deg, .0, .0).asTPose());

	PATH_T pose_path;
	const auto t0 = mrpt::Clock::now();
	using namespace std::chrono_literals;
	const auto dt = 250ms;
	auto t = t0;

	std::vector<mrpt::Clock::duration> Ats(N);
	for (long i = 0; i < N; i++)
	{
		if (INSERT_AT_END) { Ats[i] = dt; }
		else
		{
			std::chrono::duration<double> randomDuration(
				mrpt::random::getRandomGenerator().drawUniform(-5.0, 5.0));
			Ats[i] = std::chrono::duration_cast<mrpt::Clock::duration>(
				randomDuration);
		}
	}

	if (BENCHMARK_INSERT_nQUERY)
	{
		tictac.Tic();
		for (long i = 0; i < N; i++)
		{
			pose_path.insert(t, a);
			t += Ats[i];  // negligible cost...
		}
		return tictac.Tac() / N;
	}
	else
	{
		for (long i = 0; i < N; i++)
		{
			pose_path.insert(t, a);
			t += 2 * Ats[i];
		}
		t = t0 + 4512ms;  // arbitrary period

		pose_t p;
		bool valid;
		tictac.Tic();
		for (long i = 0; i < N; i++)
		{
			pose_path.interpolate(t, p, valid);
			t += Ats[i];  // negligible cost...
		}
		dummy_do_nothing_with_string(
			mrpt::format("%s %s", p.asString().c_str(), valid ? "YES" : "NO"));
		return tictac.Tac() / N;
	}
}

// ------------------------------------------------------
// register_tests_pose_interp
// ------------------------------------------------------
void register_tests_pose_interp()
{
	mrpt::random::getRandomGenerator().randomize(1234);

	using mrpt::math::TPose2D;
	using mrpt::math::TPose3D;
	using namespace mrpt::poses;

	lstTests.emplace_back(
		"CPose3DInterpolator: CPose3D insert pose at end",
		&pose_interp_test<CPose3DInterpolator, CPose3D, true, true>);
	lstTests.emplace_back(
		"CPose3DInterpolator: CPose3D insert pose random",
		&pose_interp_test<CPose3DInterpolator, CPose3D, false, true>);
	lstTests.emplace_back(
		"CPose3DInterpolator: CPose3D query",
		&pose_interp_test<CPose3DInterpolator, CPose3D, true, false>);

	lstTests.emplace_back(
		"CPose3DInterpolator: TPose3D insert pose at end",
		&pose_interp_test<CPose3DInterpolator, TPose3D, true, true>);
	lstTests.emplace_back(
		"CPose3DInterpolator: TPose3D insert pose random",
		&pose_interp_test<CPose3DInterpolator, TPose3D, false, true>);
	lstTests.emplace_back(
		"CPose3DInterpolator: TPose3D query",
		&pose_interp_test<CPose3DInterpolator, TPose3D, true, false>);

	lstTests.emplace_back(
		"CPose2DInterpolator: TPose2D insert pose at end",
		&pose_interp_test<CPose2DInterpolator, TPose2D, true, true>);
	lstTests.emplace_back(
		"CPose2DInterpolator: TPose2D insert pose random",
		&pose_interp_test<CPose2DInterpolator, TPose2D, false, true>);
	lstTests.emplace_back(
		"CPose2DInterpolator: TPose2D query",
		&pose_interp_test<CPose2DInterpolator, TPose2D, true, false>);
}
