/* +------------------------------------------------------------------------+
   |                     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/nav/planners/nav_plan_geometry_utils.h>

#include <cmath>

TEST(NavTests, NavGeomUtils_collision_straight_circ_robot)
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::nav;

	{
		TPoint2D p0(0, 0), p1(1, 1);
		double colDist;
		{
			const double R = 0.1;
			TPoint2D pObs(1.0, 0.0);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_FALSE(ret);
		}
		{
			const double R = 0.1;
			TPoint2D pObs(0.0, 1.0);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_FALSE(ret);
		}
		{
			const double R = 0.1;
			TPoint2D pObs(-0.4, -0.4);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_FALSE(ret);
		}
		{
			const double R = 0.1;
			TPoint2D pObs(1.4, 1.4);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_FALSE(ret);
		}
		{
			const double R = 0.1;
			TPoint2D pObs(0.45, 0.48);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_TRUE(ret);
		}
	}

	{
		TPoint2D p0(4, 5), p1(5, 5);
		double colDist;
		const double R = 0.5;
		{
			TPoint2D pObs(5.0, 5.0);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_TRUE(ret);
			EXPECT_NEAR(colDist, 0.5, 1e-6);
		}
		{
			TPoint2D pObs(4.510, 5.001);
			const bool ret = collision_free_dist_segment_circ_robot(
				p0, p1, R, pObs, colDist);
			EXPECT_TRUE(ret);
			EXPECT_NEAR(colDist, 0.01, 3e-3);
		}
	}
	{
		TPoint2D p0(0, 0), p1(-2, -1);
		double colDist;
		const double R = 0.5;
		TPoint2D pObs(-2.0, -1.0);
		const bool ret =
			collision_free_dist_segment_circ_robot(p0, p1, R, pObs, colDist);
		EXPECT_TRUE(ret);
		EXPECT_NEAR(colDist, std::sqrt(5.0) - 0.5, 1e-6);
	}
}
