/* +------------------------------------------------------------------------+
   |                     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/TBoundingBox.h>
#include <mrpt/math/TPose3D.h>

template <typename T>
void testDefaultCtor()
{
	mrpt::math::TBoundingBox_<T> bb;

	EXPECT_EQ(bb.min.x, 0);
	EXPECT_EQ(bb.min.y, 0);
	EXPECT_EQ(bb.min.z, 0);

	EXPECT_EQ(bb.max.x, 0);
	EXPECT_EQ(bb.max.y, 0);
	EXPECT_EQ(bb.max.z, 0);
}

TEST(TBoundingBox, defaultCtor)
{
	testDefaultCtor<float>();
	testDefaultCtor<double>();
}

template <typename T>
void testFromCorners()
{
	mrpt::math::TBoundingBox_<T> bb({-1, -2, -3}, {9, 8, 7});
	EXPECT_NEAR(bb.volume(), 10 * 10 * 10, 1e-3);

	auto bb2 = mrpt::math::TBoundingBox_<T>::FromUnsortedPoints(
		{9, 8, 7}, {-1, -2, -3});
	EXPECT_NEAR(bb2.volume(), 10 * 10 * 10, 1e-3);
}

TEST(TBoundingBox, fromCorners)
{
	testFromCorners<float>();
	testFromCorners<double>();
}

template <typename T>
void testInvalidThrows()
{
	EXPECT_ANY_THROW(mrpt::math::TBoundingBox_<T> bb({0, 0, 0}, {-1, 1, 1}));
	EXPECT_ANY_THROW(mrpt::math::TBoundingBox_<T> bb({0, 0, 0}, {1, -1, 1}));
	EXPECT_ANY_THROW(mrpt::math::TBoundingBox_<T> bb({0, 0, 0}, {1, 1, -1}));
}

TEST(TBoundingBox, invalidThrows)
{
	testInvalidThrows<float>();
	testInvalidThrows<double>();
}
template <typename T>
void testIntersections()
{
	{
		mrpt::math::TBoundingBox_<T> bb1({0, 0, 0}, {1, 1, 1});
		mrpt::math::TBoundingBox_<T> bb2({1.1, 0, 0}, {1.2, 1, 1});
		const auto inter = bb1.intersection(bb2);
		EXPECT_FALSE(inter);
	}

	{
		mrpt::math::TBoundingBox_<T> bb1({0, 0, 0}, {1, 1, 0});
		mrpt::math::TBoundingBox_<T> bb2({0.1, 0.2, 0}, {0.8, 0.9, 0});
		const T epsilon = static_cast<T>(0.001);
		const auto inter = bb1.intersection(bb2, epsilon);
		EXPECT_TRUE(inter);
	}
	{
		mrpt::math::TBoundingBox_<T> bb1({0, 0, -1e-8}, {1, 1, -1e-8});
		mrpt::math::TBoundingBox_<T> bb2({0.1, 0.2, -1e-9}, {0.8, 0.9, 2e-9});
		const T epsilon = static_cast<T>(0.001);
		const auto inter = bb1.intersection(bb2, epsilon);
		EXPECT_TRUE(inter);
	}

	{
		mrpt::math::TBoundingBox_<T> bb1({0, 1, 2}, {10, 10, 10});
		mrpt::math::TBoundingBox_<T> bb2({5, 6, 7}, {6, 7, 8});
		const auto inter = bb1.intersection(bb2);
		EXPECT_TRUE(inter);
		EXPECT_EQ(inter->min, (mrpt::math::TPoint3D_<T>{5, 6, 7}));
		EXPECT_EQ(inter->max, (mrpt::math::TPoint3D_<T>{6, 7, 8}));
	}

	{
		mrpt::math::TBoundingBox_<T> bb1({0, 1, 2}, {10, 11, 12});
		mrpt::math::TBoundingBox_<T> bb2({5, 6, 7}, {16, 17, 18});
		const auto inter = bb1.intersection(bb2);
		EXPECT_TRUE(inter);
		EXPECT_EQ(inter->min, (mrpt::math::TPoint3D_<T>{5, 6, 7}));
		EXPECT_EQ(inter->max, (mrpt::math::TPoint3D_<T>{10, 11, 12}));
	}

	{
		mrpt::math::TBoundingBox_<T> bb1({0, 1, 2}, {10, 10, 10});
		mrpt::math::TBoundingBox_<T> bb2({-5, -6, -7}, {6, 7, 8});
		const auto inter = bb1.intersection(bb2);
		EXPECT_TRUE(inter);
		EXPECT_EQ(inter->min, (mrpt::math::TPoint3D_<T>{0, 1, 2}));
		EXPECT_EQ(inter->max, (mrpt::math::TPoint3D_<T>{6, 7, 8}));
	}
}

TEST(TBoundingBox, intersections)
{
	testIntersections<float>();
	testIntersections<double>();
}

TEST(TBoundingBox, compose)
{
	mrpt::math::TBoundingBox bb1({0, 1, 2}, {3, 4, 5});
	const auto bb2 = bb1.compose(mrpt::math::TPose3D(10, 20, 30, 0, 0, 0));

	EXPECT_EQ(bb2.min, mrpt::math::TPoint3D(10, 21, 32));
	EXPECT_EQ(bb2.max, mrpt::math::TPoint3D(13, 24, 35));
}

TEST(TBoundingBox, inverseCompose)
{
	using namespace mrpt;

	mrpt::math::TBoundingBox bb1({0, 1, 2}, {3, 4, 5});
	{
		const auto bb2 =
			bb1.inverseCompose(mrpt::math::TPose3D(10, 20, 30, 0, 0, 0));

		EXPECT_EQ(bb2.min, mrpt::math::TPoint3D(-10, -19, -28));
		EXPECT_EQ(bb2.max, mrpt::math::TPoint3D(-7, -16, -25));
	}
	{
		const auto bb2 = bb1.inverseCompose(
			mrpt::math::TPose3D(10, 20, 30, 180.0_deg, 0, 0));

		EXPECT_NEAR(
			(bb2.min - mrpt::math::TPoint3D(7.0, 16.0, -28.0)).norm(), 0.0,
			1e-4)
			<< bb2.min << std::endl;
		EXPECT_NEAR(
			(bb2.max - mrpt::math::TPoint3D(10.0, 19.0, -25.0)).norm(), 0.0,
			1e-4)
			<< bb2.max << std::endl;
	}
}
