#ifndef OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
#define OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_

#include <cstddef>
#include <optional>
#include <string>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/msg/path.hpp"

namespace obstacle_nav2
{

struct CollisionCheckResult
{
  bool blocked{false};
  std::size_t path_index{0};
  std::string reason{"clear"};
};

struct GuardSettings
{
  double lookahead_distance{2.0};
  double rejoin_min_distance{1.0};
  double rejoin_max_distance{4.0};
  int occupied_threshold{50};
  bool treat_unknown_as_occupied{true};
  double footprint_half_length{0.14};
  double footprint_half_width{0.085};
  double footprint_padding{0.04};
  double footprint_sample_step{0.05};
};

double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);

double distance2d(
  const geometry_msgs::msg::PoseStamped & a,
  const geometry_msgs::msg::PoseStamped & b);

std::size_t nearestPathIndex(
  const nav_msgs::msg::Path & path,
  const geometry_msgs::msg::PoseStamped & pose);

std::size_t advanceByDistance(
  const nav_msgs::msg::Path & path,
  std::size_t start_index,
  double distance_m);

CollisionCheckResult checkPathAhead(
  const nav_msgs::msg::Path & path,
  std::size_t start_index,
  const nav_msgs::msg::OccupancyGrid & costmap,
  const GuardSettings & settings);

std::optional<std::size_t> findClearRejoinIndex(
  const nav_msgs::msg::Path & path,
  std::size_t start_index,
  const nav_msgs::msg::OccupancyGrid & costmap,
  const GuardSettings & settings);

std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
  const nav_msgs::msg::Path & path,
  std::size_t start_index,
  const nav_msgs::msg::OccupancyGrid & local_costmap,
  const nav_msgs::msg::OccupancyGrid * planner_costmap,
  const GuardSettings & settings);

bool shouldRetryBlockedRepair(
  const std::optional<std::size_t> & last_repair_nearest_index,
  std::size_t nearest_index,
  int min_repair_progress_indices,
  double seconds_since_last_repair,
  double blocked_retry_wait_sec,
  bool costmap_updated_since_repair);

nav_msgs::msg::Path slicePath(
  const nav_msgs::msg::Path & path,
  std::size_t start_index,
  std::size_t end_index_inclusive);

nav_msgs::msg::Path stitchPaths(
  const nav_msgs::msg::Path & bypass,
  const nav_msgs::msg::Path & original,
  std::size_t rejoin_index);

}  // namespace obstacle_nav2

#endif  // OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
