/* +------------------------------------------------------------------------+
   |                     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/io/CFileGZInputStream.h>
#include <mrpt/nav/planners/PlannerSimple2D.h>
#include <mrpt/serialization/CArchive.h>
#include <test_mrpt_common.h>

TEST(PlannerSimple2D, findPath)
{
  using namespace std::string_literals;

  const auto fil = mrpt::UNITTEST_BASEDIR() + "/share/mrpt/datasets/2006-MalagaCampus.gridmap.gz"s;

  // Load the gridmap:
  mrpt::maps::COccupancyGridMap2D gridmap;

  {
    mrpt::io::CFileGZInputStream f(fil);
    auto arch = mrpt::serialization::archiveFrom(f);
    arch >> gridmap;
  }

  // Find path:
  mrpt::nav::PlannerSimple2D pathPlanning;
  pathPlanning.robotRadius = 0.30f;

  {
    std::deque<mrpt::math::TPoint2D> thePath;
    bool notFound;
    const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
    pathPlanning.computePath(gridmap, origin, target, thePath, notFound);

    EXPECT_FALSE(notFound);
    EXPECT_EQ(thePath.size(), 416U);
    EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
    EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
    EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
    EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
  }
  {
    std::deque<mrpt::math::TPoint2D> thePath;
    bool notFound;
    const mrpt::poses::CPose2D origin(90, 40, 0), target(20, -110, 0);
    pathPlanning.computePath(
        gridmap, origin, target, thePath, notFound, 300.0f /* Max. distance */);

    EXPECT_FALSE(notFound);
    EXPECT_EQ(thePath.size(), 416U);
    EXPECT_NEAR(thePath.at(0).x, origin.x(), 1.0);
    EXPECT_NEAR(thePath.at(0).y, origin.y(), 1.0);
    EXPECT_NEAR(thePath.back().x, target.x(), 1.0);
    EXPECT_NEAR(thePath.back().y, target.y(), 1.0);
  }

  {
    std::deque<mrpt::math::TPoint2D> thePath;
    bool notFound;
    const mrpt::poses::CPose2D origin(20, -110, 0), target(90, 40, 0);
    pathPlanning.computePath(gridmap, origin, target, thePath, notFound, 10.0f /* Max. distance */);

    EXPECT_TRUE(notFound);
  }

  {
    std::deque<mrpt::math::TPoint2D> thePath;
    bool notFound;
    const mrpt::poses::CPose2D origin(20, -110, 0), target(900, 40, 0);
    pathPlanning.computePath(
        gridmap, origin, target, thePath, notFound, 100.0f /* Max. distance */);

    EXPECT_TRUE(notFound);
    EXPECT_EQ(thePath.size(), 0U);
  }
}
