#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_

#include <optional>
#include <string>
#include <unordered_map>
#include <vector>

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

namespace racing_control
{

struct CandidateWaypoint
{
  std::string group_name;
  std::string source_file;
  std::string point_name;
  geometry_msgs::msg::PoseStamped pose;
};

using CandidateWaypointGroups = std::unordered_map<std::string, std::vector<CandidateWaypoint>>;

struct RecoveryCommand
{
  double linear_x{0.0};
  double angular_z{0.0};
  double duration_sec{0.0};
  std::string label;
};

CandidateWaypointGroups loadCandidateWaypointGroups(
  const std::string & json_dir,
  const std::string & frame_id);

std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
  const nav_msgs::msg::OccupancyGrid & costmap,
  const geometry_msgs::msg::PoseStamped & current,
  int occupied_threshold,
  bool treat_unknown_as_occupied,
  double search_radius_m = 1.0);

std::optional<RecoveryCommand> rearClearRecoveryCommand(
  const nav_msgs::msg::OccupancyGrid & costmap,
  const geometry_msgs::msg::PoseStamped & current,
  int occupied_threshold,
  bool treat_unknown_as_occupied,
  double rear_clear_distance_m);

class CandidateWaypointSelector
{
public:
  void setGroups(CandidateWaypointGroups groups);
  void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap);

  std::optional<CandidateWaypoint> select(
    const std::string & group_name,
    int occupied_threshold,
    bool treat_unknown_as_occupied,
    const std::string & preferred_source_file = "") const;

  std::optional<RecoveryCommand> nearestFreeRecoveryCommand(
    const geometry_msgs::msg::PoseStamped & current,
    int occupied_threshold,
    bool treat_unknown_as_occupied,
    double search_radius_m = 1.0) const;

  bool isPoseOccupied(
    const geometry_msgs::msg::PoseStamped & pose,
    int occupied_threshold,
    bool treat_unknown_as_occupied) const;

private:
  bool isCandidateOccupied(
    const geometry_msgs::msg::PoseStamped & pose,
    int occupied_threshold,
    bool treat_unknown_as_occupied) const;

  std::optional<RecoveryCommand> commandForRecoveryAngle(double angle_rad) const;

  CandidateWaypointGroups groups_;
  nav_msgs::msg::OccupancyGrid costmap_;
  bool has_costmap_{false};
};

}  // namespace racing_control

#endif  // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_
