/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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);
  }
}
