/* +------------------------------------------------------------------------+
   |                     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/math/robust_kernels.h>

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

// Test data tables:
// Lists of: E2 (squared error) ;  PARAM2 (squared kernel parameter)  ;  R2
// (expected robustified output)   ; R' (1st derivative)   ; R'' (2nd
// derivative)
// =============  Kernel: None (plain least squares)
const double list_test_kernel_none[][5] = {
	{0.0, .0, 0.0, 1.0, 0.0},
	{1.0, .0, 1.0, 1.0, 0.0},
	{10.0, .0, 10.0, 1.0, 0.0}};

// =============  Kernel: Pseudo-Huber
const double list_test_kernel_pshb[][5] = {
	{0.0, 1.0, 0.0, 1.0, -0.5},
	{0.0, 4.0, 0.0, 1.0, -0.125},
	{0.0, 9.0, 0.0, 1.0, -0.0555556},
	{1.0, 1.0, 0.828427, 0.707107, -0.176777},
	{1.0, 4.0, 0.944272, 0.894427, -0.0894427},
	{1.0, 9.0, 0.973666, 0.948683, -0.0474342},
	{4.0, 1.0, 2.47214, 0.447214, -0.0447214},
	{4.0, 4.0, 3.31371, 0.707107, -0.0441942},
	{4.0, 9.0, 3.63331, 0.83205, -0.0320019}};

template <TRobustKernelType KERNEL_TYPE>
void tester_robust_kernel(const double table[][5], const size_t N)
{
	RobustKernel<KERNEL_TYPE> rKernel;

	for (size_t i = 0; i < N; i++)
	{
		const double e2 = table[i][0];
		const double param2 = table[i][1];

		double rhop, rhopp;
		rKernel.param_sq = param2;
		double r2 = rKernel.eval(e2, rhop, rhopp);

		const double expected_r2 = table[i][2];
		const double expected_rhop = table[i][3];
		const double expected_rhopp = table[i][4];

		EXPECT_NEAR(r2, expected_r2, 1e-5);
		EXPECT_NEAR(rhop, expected_rhop, 1e-5);
		EXPECT_NEAR(rhopp, expected_rhopp, 1e-5);
	}
}

TEST(RobustKernels, PlainLeastSquares)
{
	const size_t N =
		sizeof(list_test_kernel_none) / sizeof(list_test_kernel_none[0]);
	tester_robust_kernel<rkLeastSquares>(list_test_kernel_none, N);
}

TEST(RobustKernels, PseudoHuber)
{
	const size_t N =
		sizeof(list_test_kernel_pshb) / sizeof(list_test_kernel_pshb[0]);
	tester_robust_kernel<rkPseudoHuber>(list_test_kernel_pshb, N);
}
