#include <gtest/gtest.h>

#include "obstacle_nav2/trajectory_guard.hpp"

namespace
{

geometry_msgs::msg::PoseStamped pose(double x, double y)
{
  geometry_msgs::msg::PoseStamped p;
  p.header.frame_id = "odom";
  p.pose.position.x = x;
  p.pose.position.y = y;
  p.pose.orientation.w = 1.0;
  return p;
}

nav_msgs::msg::Path straightPath()
{
  nav_msgs::msg::Path path;
  path.header.frame_id = "odom";
  for (int i = 0; i <= 10; ++i) {
    path.poses.push_back(pose(0.2 * i, 0.0));
  }
  return path;
}

nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y)
{
  nav_msgs::msg::OccupancyGrid grid;
  grid.header.frame_id = "odom";
  grid.info.resolution = 0.1;
  grid.info.width = 40;
  grid.info.height = 30;
  grid.info.origin.position.x = origin_x;
  grid.info.origin.position.y = origin_y;
  grid.data.assign(grid.info.width * grid.info.height, 0);
  return grid;
}

void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my)
{
  ASSERT_GE(mx, 0);
  ASSERT_GE(my, 0);
  ASSERT_LT(mx, static_cast<int>(grid.info.width));
  ASSERT_LT(my, static_cast<int>(grid.info.height));
  grid.data[my * grid.info.width + mx] = 100;
}

}  // namespace

TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose)
{
  const auto path = straightPath();
  EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u);
}

TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength)
{
  const auto path = straightPath();
  EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u);
}

TEST(TrajectoryGuard, SlicePathIncludesEndIndex)
{
  const auto path = straightPath();
  const auto sliced = obstacle_nav2::slicePath(path, 2, 4);
  ASSERT_EQ(sliced.poses.size(), 3u);
  EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4);
  EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8);
}

TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex)
{
  const auto original = straightPath();
  nav_msgs::msg::Path bypass;
  bypass.header.frame_id = "odom";
  bypass.poses.push_back(pose(0.0, 0.0));
  bypass.poses.push_back(pose(0.5, 0.3));

  const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4);
  ASSERT_EQ(stitched.poses.size(), 9u);
  EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0);
  EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3);
  EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8);
}

TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell)
{
  auto path = straightPath();
  auto grid = gridWithOrigin(-1.0, -1.0);
  markOccupied(grid, 15, 10);

  obstacle_nav2::GuardSettings settings;
  settings.lookahead_distance = 2.0;
  settings.occupied_threshold = 50;
  settings.footprint_half_length = 0.01;
  settings.footprint_half_width = 0.01;
  settings.footprint_padding = 0.0;

  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
  EXPECT_TRUE(result.blocked);
  EXPECT_EQ(result.reason, "occupied");
}

TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked)
{
  auto path = straightPath();
  auto grid = gridWithOrigin(-1.0, -1.0);

  obstacle_nav2::GuardSettings settings;
  settings.lookahead_distance = 2.0;
  settings.occupied_threshold = 50;
  settings.footprint_half_length = 0.01;
  settings.footprint_half_width = 0.01;
  settings.footprint_padding = 0.0;

  const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
  EXPECT_FALSE(result.blocked);
  EXPECT_EQ(result.reason, "clear");
}

TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
{
  auto path = straightPath();
  auto grid = gridWithOrigin(-1.0, -1.0);
  for (int mx = 13; mx <= 18; ++mx) {
    markOccupied(grid, mx, 10);
  }

  obstacle_nav2::GuardSettings settings;
  settings.rejoin_min_distance = 0.5;
  settings.rejoin_max_distance = 2.0;
  settings.occupied_threshold = 50;
  settings.footprint_half_length = 0.01;
  settings.footprint_half_width = 0.01;
  settings.footprint_padding = 0.0;

  const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings);
  ASSERT_TRUE(rejoin.has_value());
  EXPECT_GE(*rejoin, 5u);
}

TEST(TrajectoryGuard, FindClearRejoinIndexCanUsePlannerCostmapBeyondLocalWindow)
{
  auto path = straightPath();
  auto local_grid = gridWithOrigin(-0.5, -1.0);
  local_grid.info.width = 15;
  local_grid.data.assign(local_grid.info.width * local_grid.info.height, 0);

  auto planner_grid = gridWithOrigin(-1.0, -1.0);
  planner_grid.info.width = 80;
  planner_grid.data.assign(planner_grid.info.width * planner_grid.info.height, 0);

  obstacle_nav2::GuardSettings settings;
  settings.rejoin_min_distance = 1.5;
  settings.rejoin_max_distance = 3.0;
  settings.occupied_threshold = 50;
  settings.treat_unknown_as_occupied = true;
  settings.footprint_half_length = 0.01;
  settings.footprint_half_width = 0.01;
  settings.footprint_padding = 0.0;

  EXPECT_FALSE(obstacle_nav2::findClearRejoinIndex(path, 0, local_grid, settings).has_value());

  const auto rejoin = obstacle_nav2::findClearRejoinIndexWithPlannerCostmap(
    path, 0, local_grid, &planner_grid, settings);
  ASSERT_TRUE(rejoin.has_value());
  EXPECT_GE(*rejoin, 8u);
}

TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
{
  const std::optional<std::size_t> last_repair_index = 10u;

  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
    last_repair_index, 11u, 5, 0.8, 1.0, true));
  EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
    last_repair_index, 11u, 5, 1.2, 1.0, false));
  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
    last_repair_index, 11u, 5, 1.2, 1.0, true));
}

TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately)
{
  const std::optional<std::size_t> last_repair_index = 10u;

  EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
    last_repair_index, 16u, 5, 0.1, 1.0, false));
}