/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/core/bits_math.h>  // DEG2RAD
#include <mrpt/math/CAtan2LookUpTable.h>
#include <mrpt/random.h>

#include <cmath>

template <class LUT_CLASS>
void atan2_lut_test(
	const LUT_CLASS& atan2lut, const double SIZE, const double max_deg_errors,
	const double skip_area)
{
	for (int i = 0; i < 1000; i++)
	{
		const double x = mrpt::random::getRandomGenerator().drawUniform(
			-.5 * SIZE, .5 * SIZE);
		const double y = mrpt::random::getRandomGenerator().drawUniform(
			-.5 * SIZE, .5 * SIZE);

		// Avoid the central part, where accuracy is worse
		if (std::abs(x) < skip_area || std::abs(y) < skip_area) continue;

		const double atan2_good = ::atan2(y, x);
		double atan2_lut = .0;
		bool atan2_lut_valid = atan2lut.atan2(y, x, atan2_lut);

		EXPECT_TRUE(atan2_lut_valid);
		EXPECT_LT(
			std::abs(atan2_good - atan2_lut), mrpt::DEG2RAD(max_deg_errors))
			<< "(x,y): (" << x << " , " << y << ")"
			<< "\natan2_good:" << atan2_good << " atan2_lut:" << atan2_lut
			<< std::endl;
	}
}

TEST(CAtan2LookUpTable, ValidValidTest)
{
	const double SIZE = 20.0;
	const double RES = 0.10;
	mrpt::math::CAtan2LookUpTable atan2lut(
		-.5 * SIZE, .5 * SIZE, -.5 * SIZE, .5 * SIZE, RES);
	atan2_lut_test(atan2lut, SIZE, 1.5 /*max error*/, 2.0 /* skip zone */);
}

TEST(CAtan2LookUpTable, MultiResTest)
{
	const double SIZE = 20.0;
	mrpt::math::CAtan2LookUpTableMultiRes atan2lut;

	std::map<double, double> res2extension;
	res2extension[0.001] = 0.8;	 // 0.1 cm resolution
	res2extension[0.01] = 2.0;	// 1.0 cm resolution
	res2extension[0.02] = 5.0;	// 2.0 cm resolution
	res2extension[0.05] = 11.0;	 // 5.0 cm resolution
	atan2lut.resize(res2extension);

	atan2_lut_test(atan2lut, SIZE, 0.5 /*max error*/, 0.4 /* skip zone */);
}
