/* +------------------------------------------------------------------------+
   |                     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 <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/transform_gaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

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

template class mrpt::CTraitsTest<CPosePDFGaussian>;

class PosePDFGaussTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	static CPosePDFGaussian generateRandomPose2DPDF(
		double x, double y, double phi, double std_scale)
	{
		CMatrixDouble31 r;
		mrpt::random::getRandomGenerator().drawGaussian1DMatrix(
			r, 0, std_scale);
		CMatrixDouble33 cov;
		cov.matProductOf_AAt(r);  // random semi-definite positive matrix:
		for (int i = 0; i < 3; i++)
			cov(i, i) += 1e-7;
		CPosePDFGaussian pdf(CPose2D(x, y, phi), cov);
		return pdf;
	}

	static void func_inverse(
		const CVectorFixedDouble<3>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<3>& Y)
	{
		const CPose2D p1(x[0], x[1], x[2]);
		const CPose2D p1_inv = CPose2D() - p1;
		for (int i = 0; i < 3; i++)
			Y[i] = p1_inv[i];
	}

	void testPoseInverse(double x, double y, double phi, double std_scale)
	{
		CPosePDFGaussian pdf1 = generateRandomPose2DPDF(x, y, phi, std_scale);

		CPosePDFGaussian pdf1_inv;
		pdf1.inverse(pdf1_inv);

		// Numeric approximation:
		CVectorFixedDouble<3> y_mean;
		CMatrixFixed<double, 3, 3> y_cov;
		{
			CVectorFixedDouble<3> x_mean;
			for (int i = 0; i < 3; i++)
				x_mean[i] = pdf1.mean[i];

			CMatrixFixed<double, 3, 3> x_cov = pdf1.cov;

			double DUMMY = 0;
			CVectorFixedDouble<3> x_incrs;
			x_incrs.fill(1e-6);
			transform_gaussian_linear(
				x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
		}
		// Compare:
		EXPECT_NEAR(0, (y_cov - pdf1_inv.cov).array().abs().mean(), 1e-5)
			<< "pdf1 mean: " << pdf1.mean << endl
			<< "Numeric approximation of covariance: " << endl
			<< y_cov << endl
			<< "Returned covariance: " << endl
			<< pdf1_inv.cov << endl;
	}
};

TEST_F(PosePDFGaussTests, Inverse)
{
	testPoseInverse(0, 0, 0, 0.01);
	testPoseInverse(0, 0, 0, 0.1);

	testPoseInverse(1, 0, 0, 0.1);
	testPoseInverse(0, 1, 0, 0.1);
	testPoseInverse(0, 0, 1, 0.1);

	testPoseInverse(-5, 0, 0, 0.1);
	testPoseInverse(0, -5, 0, 0.1);
	testPoseInverse(0, 0, -5, 0.1);

	testPoseInverse(4, 6, 10.0_deg, 0.1);
	testPoseInverse(4, 6, -10.0_deg, 0.1);

	testPoseInverse(-7, 2, 30.0_deg, 0.1);
	testPoseInverse(-7, 2, -30.0_deg, 0.1);
}
