/* +------------------------------------------------------------------------+
   |                     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/data_utils.h>
#include <mrpt/math/distributions.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

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

const double eps = 1e-12;

TEST(distributions, normalPDF_1d)
{
	EXPECT_NEAR(normalPDF(0, 0, 1), 0.398942280401433, eps);
	EXPECT_NEAR(normalPDF(5, 5, 1), 0.398942280401433, eps);
	EXPECT_NEAR(normalPDF(0, 0, 2), 0.199471140200716, eps);
	EXPECT_NEAR(normalPDF(1, 0, 1), 0.241970724519143, eps);
}

TEST(distributions, normalPDF_vector)
{
	const double cov_vals[3 * 3] = {4.0, 2.0, 1.0, 2.0, 3.0,
									0.5, 1.0, 0.5, 1.0};
	const double x1_vals[3] = {1.0, 0.0, 0.0};
	const double x2_vals[3] = {1.0, 2.0, 3.0};

	const CMatrixDouble33 COV(cov_vals);
	const CMatrixFixed<double, 3, 1> x0;
	const CMatrixFixed<double, 3, 1> x1(x1_vals);
	const CMatrixFixed<double, 3, 1> x2(x2_vals);

	EXPECT_NEAR(
		normalPDF(x0, x0, COV), 0.02592116832548877620,
		eps);  // sprintf('%.20f',mvnpdf([0;0;0],[0;0;0],S))
	EXPECT_NEAR(
		normalPDF(x2, x2, COV), 0.02592116832548877620,
		eps);  // sprintf('%.20f',mvnpdf([0;0;0],[0;0;0],S))

	EXPECT_NEAR(
		normalPDF(x1, x0, COV), 0.02061240910323311470,
		eps);  // sprintf('%.20f',mvnpdf([1;0;0],[0;0;0],S))
	EXPECT_NEAR(
		normalPDF(x2, x0, COV), 0.00008423820480102986,
		eps);  // sprintf('%.20f',mvnpdf([1;2;3],[0;0;0],S))

	EXPECT_NEAR(
		normalPDF(x1, COV), 0.02061240910323311470,
		eps);  // sprintf('%.20f',mvnpdf([1;0;0],[0;0;0],S))
	EXPECT_NEAR(
		normalPDF(x2, COV), 0.00008423820480102986,
		eps);  // sprintf('%.20f',mvnpdf([1;0;0],[0;0;0],S))
}

TEST(distributions, erfc)
{
	const double eps2 = 1e-7;

	EXPECT_NEAR(std::erfc(0), 1, eps);
	EXPECT_NEAR(std::erfc(1), 0.157299207050285, eps2);
	EXPECT_NEAR(std::erfc(2), 0.004677734981047, eps2);
}

TEST(distributions, erf)
{
	const double eps2 = 1e-7;

	EXPECT_NEAR(std::erf(0), 0, eps);
	EXPECT_NEAR(std::erf(1), 0.842700792949715, eps2);
	EXPECT_NEAR(std::erf(2), 0.995322265018953, eps2);
}

TEST(distributions, normalCDF)
{
	EXPECT_NEAR(mrpt::math::normalCDF(0), 0.5, eps);
	EXPECT_NEAR(mrpt::math::normalCDF(1), 0.841344746068543, eps);
	EXPECT_NEAR(mrpt::math::normalCDF(2), 0.977249868051821, eps);
	EXPECT_NEAR(mrpt::math::normalCDF(3), 0.998650101968370, eps);
}

TEST(distributions, chi2inv)
{
	EXPECT_NEAR(mrpt::math::chi2inv(0.0, 1), 0, eps);
	EXPECT_NEAR(mrpt::math::chi2inv(0.5, 3), 2.365973884375338, 0.1);
	EXPECT_NEAR(mrpt::math::chi2inv(0.95, 3), 7.814727903251178, 0.1);
}

TEST(distributions, chi2PDF)
{
	EXPECT_NEAR(mrpt::math::chi2PDF(1, 1.0), 0.241970724519143, eps);
	EXPECT_NEAR(mrpt::math::chi2PDF(1, 2.0), 0.103776874355149, eps);
	EXPECT_NEAR(mrpt::math::chi2PDF(1, 3.0), 0.051393443267923, eps);
	EXPECT_NEAR(mrpt::math::chi2PDF(1, 4.0), 0.026995483256594, eps);

	EXPECT_NEAR(mrpt::math::chi2PDF(4, 1.0), 0.151632664928158, eps);
}

TEST(distributions, noncentralChi2PDF_CDF)
{
	const double eps2 = 1e-7;

	// ncx2cdf(arg,degreesOfFreedom,noncentrality)
	// noncentralChi2PDF_CDF(degreesOfFreedom,noncentrality,arg)
	EXPECT_NEAR(mrpt::math::noncentralChi2PDF_CDF(1, 1.0, 0).first, 0, eps2);
	EXPECT_NEAR(mrpt::math::noncentralChi2PDF_CDF(1, 2.0, 0).first, 0, eps2);

	// MATLAB: ncx2cdf(1:3,1,1)
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 1.0).second, 0.477249868051821,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 2.0).second, 0.652756536682270,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 3.0).second, 0.764784149631031,
		eps2);
	// MATLAB: ncx2pdf(1:3,1,1)
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 1.0).first, 0.226466623457311,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 2.0).first, 0.137103272271503,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(1, 1, 3.0).first, 0.090852330823658,
		eps2);

	// MATLAB: ncx2cdf(1:3,2,3)
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 1.0).second, 0.121825497229364,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 2.0).second, 0.252206942426039,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 3.0).second, 0.378499822919087,
		eps2);
	// MATLAB: ncx2pdf(1:3,2,3)
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 1.0).first, 0.128765424775546,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 2.0).first, 0.129923687128879,
		eps2);
	EXPECT_NEAR(
		mrpt::math::noncentralChi2PDF_CDF(2, 3, 3.0).first, 0.121500177080913,
		eps2);
}

TEST(data_utils, mahalanobisDistance2AndLogPDF)
{
	const double cov_vals[3 * 3] = {0.00393682,	  -6.11165e-07, -8.62169e-05,
									-6.11165e-07, 7.44917e-05,	-1.17274e-07,
									-8.62169e-05, -1.17274e-07, 0.000108955};
	const CMatrixDouble33 COV(cov_vals);

	const double x_vals[3] = {0.0135442, 0.00504134, -0.000452334};
	const CMatrixDouble31 x(x_vals);

	double out_maha2, out_ml;
	mrpt::math::mahalanobisDistance2AndLogPDF(x, COV, out_maha2, out_ml);

	EXPECT_NEAR(out_maha2, 0.388264, 1e-4);
	EXPECT_NEAR(out_ml, 9.14118, 1e-4);
}
