/* +------------------------------------------------------------------------+
   |                     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 <gtest/gtest.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/system/filesystem.h>
#include <test_mrpt_common.h>

TEST(NavTests, PTGs_tests)
{
	using namespace std;
	using namespace mrpt;
	using namespace mrpt::nav;

	const string sFil =
		mrpt::UNITTEST_BASEDIR + string("/tests/PTGs_for_tests.ini");
	if (!mrpt::system::fileExists(sFil))
	{
		cerr << "**WARNING* Skipping tests since file cannot be found: '"
			 << sFil << "'\n";
		return;
	}

	mrpt::config::CConfigFile cfg(sFil);

	const unsigned int PTG_COUNT =
		cfg.read_int("PTG_UNIT_TESTS", "PTG_COUNT", 0, true);
	EXPECT_TRUE(PTG_COUNT > 0);
	vector<CParameterizedTrajectoryGenerator::Ptr> PTGs(PTG_COUNT);

	for (unsigned int n = 0; n < PTG_COUNT; n++)
	{
		// Factory:
		const string sPTGName = cfg.read_string(
			"PTG_UNIT_TESTS", format("PTG%u_Type", n), "", true);
		PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
			sPTGName, cfg, "PTG_UNIT_TESTS", format("PTG%u_", n));
		EXPECT_TRUE(PTGs[n] != nullptr) << "Failed creating PTG #" << n << endl;

		try
		{
			PTGs[n]->initialize(string(), false /*verbose */);
		}
		catch (const std::exception& e)
		{
			GTEST_FAIL() << "Failed initializing PTG #" << n << endl
						 << e.what() << endl;
		}
	}

	// Run tests:
	// ---------------------------------------------------------
	for (unsigned int n = 0; n < PTG_COUNT; n++)
	{
		CParameterizedTrajectoryGenerator::Ptr ptg = PTGs[n];

		const std::string sPTGDesc = ptg->getDescription();
		const double refDist = ptg->getRefDistance();
		const size_t num_paths = ptg->getPathCount();
		size_t num_tests_run = 0;

		// TEST: step <-> dist match
		{
			for (double dist = 0.1; dist < refDist * 0.5; dist += 0.2)
			{
				bool any_good = false;
				for (size_t k = 0; k < num_paths; k++)
				{
					uint32_t step;

					if (ptg->getPathStepForDist(k, dist, step))
					{
						any_good = true;
						double d = ptg->getPathDist(k, step);
						EXPECT_NEAR(d, dist, 0.05)
							<< "Test: step <-> dist match\n PTG: " << sPTGDesc
							<< endl
							<< "dist:" << dist << endl;
						num_tests_run++;
					}
				}
				EXPECT_TRUE(any_good)
					<< "Test: step <-> dist match\n PTG: " << sPTGDesc << endl
					<< "dist:" << dist << endl;
			}
		}

		// TEST: inverseMap_WS2TP
		{
			bool any_ok = false;
			bool skip_this_ptg = false;
			for (double tx = -refDist * 0.5;
				 !skip_this_ptg && tx < refDist * 0.5; tx += 0.1)
			{
				for (double ty = -refDist * 0.5;
					 !skip_this_ptg && ty < refDist * 0.5; ty += 0.1)
				{
					if (std::abs(tx) < 1e-2 && std::abs(ty) < 1e-2)
						continue;  // TP-Space does not include the WS point
					// (0,0) in its domain

					const double tolerance_dist = std::max(
						0.10,
						10.0 * std::sqrt(tx * tx + ty * ty) * M_PI * 2 /
							ptg->getPathCount());

					int k;
					double normalized_d;
					bool valid = ptg->inverseMap_WS2TP(tx, ty, k, normalized_d);
					if (valid && normalized_d < 1.0)
					{
						any_ok = true;
						// Now, do the inverse operation:
						uint32_t step;
						bool step_ok = ptg->getPathStepForDist(
							k, normalized_d * refDist, step);
						EXPECT_TRUE(step_ok)
							<< "PTG: " << sPTGDesc << endl
							<< "(tx,ty): " << tx << " " << ty << " k= " << k
							<< " normalized_d=" << normalized_d << endl;
						if (step_ok)
						{
							const mrpt::math::TPose2D pose =
								ptg->getPathPose(k, step);
							EXPECT_NEAR(pose.x, tx, tolerance_dist)
								<< "Test: inverseMap_WS2TP\n PTG#" << n << ": "
								<< sPTGDesc << endl
								<< "(tx,ty): " << tx << " " << ty << " k= " << k
								<< " normalized_d=" << normalized_d << endl;
							EXPECT_NEAR(pose.y, ty, tolerance_dist)
								<< "Test: inverseMap_WS2TP\n PTG#" << n << ": "
								<< sPTGDesc << endl
								<< "(tx,ty): " << tx << " " << ty << " k= " << k
								<< " normalized_d=" << normalized_d << endl;

							if (std::abs(pose.x - tx) >= tolerance_dist ||
								std::abs(pose.y - ty) >= tolerance_dist)
								skip_this_ptg = true;
							else
								num_tests_run++;
						}
					}
				}
			}
			EXPECT_TRUE(any_ok) << "PTG: " << sPTGDesc << endl;
		}

		// TEST: TP_obstacles
		{
			bool skip_this_ptg = false;
			bool any_change_all = false;
			for (double ox = -refDist * 0.5;
				 !skip_this_ptg && ox < refDist * 0.5; ox += 0.1)
			{
				for (double oy = -refDist * 0.5;
					 !skip_this_ptg && oy < refDist * 0.5; oy += 0.1)
				{
					if (std::abs(ox) < 1e-2 && std::abs(oy) < 1e-2)
						continue;  // TP-Space does not include the WS point
					// (0,0) in its domain

					std::vector<double> TP_obstacles;
					ptg->initTPObstacles(TP_obstacles);

					const std::vector<double> TP_obstacles_org = TP_obstacles;
					ptg->updateTPObstacle(ox, oy, TP_obstacles);

					const bool any_change = (TP_obstacles_org != TP_obstacles);
					if (any_change) any_change_all = true;
					num_tests_run++;
				}
			}
			EXPECT_TRUE(any_change_all);
		}

		printf(
			"PTG `%50s` run %6u tests.\n", sPTGDesc.c_str(),
			(unsigned int)num_tests_run);

	}  // for each ptg
}
