/* +------------------------------------------------------------------------+
   |                     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/math/wrap2pi.h>
#include <mrpt/poses/SO_SE_average.h>

#include <Eigen/Dense>

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

void run_test_so2_avrg(
	const double* angs, const size_t N, const double ang_correct_avr)
{
	SO_average<2> so_avr;
	for (size_t i = 0; i < N; i++)
		so_avr.append(angs[i]);
	const double calc_avr = so_avr.get_average();
	EXPECT_NEAR(mrpt::math::wrapToPi(ang_correct_avr - calc_avr), .0, 1e-6);
}

TEST(SE2_SE3_avrg, SO2_average)
{
	// Simple tests:
	{
		const double angs[] = {.1};
		const double ang_correct_avr = .1;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {.0, M_PI};
		const double ang_correct_avr = .5 * M_PI;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {-0.75 * M_PI, 0.75 * M_PI};
		const double ang_correct_avr = 1.0 * M_PI;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	{
		const double angs[] = {-0.75 * M_PI, 0.75 * M_PI, 0.3 * M_PI};
		// const double angs_w[] = {1.0, 1.0, 0.1 };
		const double ang_correct_avr = 2.3668403111754515;
		run_test_so2_avrg(
			angs, sizeof(angs) / sizeof(angs[0]), ang_correct_avr);
	}
	// Test launching an exception when there is no data:
	{
		const double dummy[] = {0.};
		try
		{
			run_test_so2_avrg(dummy, 0, 0);
			GTEST_FAIL()
				<< "An exception should have been raised before this point!!";
		}
		catch (std::exception&)
		{
			// This error is expected, it's OK.
		}
	}
}

void run_test_so3_avrg(
	const double* angs, const size_t N,
	const mrpt::math::CMatrixDouble33& correct_avr)
{
	SO_average<3> so_avr;
	for (size_t i = 0; i < N; i++)
	{
		mrpt::poses::CPose3D rot(
			0, 0, 0, angs[3 * i + 0], angs[3 * i + 1], angs[3 * i + 2]);
		so_avr.append(rot.getRotationMatrix());
	}
	const auto calc_avr = so_avr.get_average();
	EXPECT_NEAR((correct_avr - calc_avr).array().abs().sum(), .0, 1e-5);
}

TEST(SE2_SE3_avrg, SO3_average)
{
	// Simple tests:
	{
		const double angs[] = {.0, .0, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {-.75 * M_PI, .0, .0, .75 * M_PI, .0, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {.0, -0.2, .0, .0, 0.2, .0};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
	{
		const double angs[] = {.0, .0, .3, .0, .0, -.3};
		const auto correct_avr =
			mrpt::poses::CPose3D(0, 0, 0, 0, 0, 0).getRotationMatrix();
		run_test_so3_avrg(
			angs, sizeof(angs) / (3 * sizeof(angs[0])), correct_avr);
	}
}
