#pragma once

#include <nlohmann/json.hpp>

#include <optional>
#include <string>
#include <utility>
#include <vector>

namespace lm {

struct WorldPoint
{
  double x = 0;
  double y = 0;
};

class PathPlanner
{
public:
  static std::optional<WorldPoint> positionWorld(const nlohmann::json& map, const std::string& position_id);
  static nlohmann::json plan(const nlohmann::json& map,
                             const WorldPoint& from,
                             const WorldPoint& to);

private:
  struct GridCell
  {
    int x = 0;
    int y = 0;
  };

  static double cellCost(const nlohmann::json& map, double wx, double wy, double img_h);
  static std::pair<double, double> worldToPixel(const nlohmann::json& map, double wx, double wy, double img_h);
  static WorldPoint pixelToWorld(const nlohmann::json& map, double px, double py, double img_h);
  static bool pointInPolygon(double px, double py, const nlohmann::json& points);
  static bool pointNearPolyline(double px, double py, const nlohmann::json& points, double half_width);
  static double zoneTraversalCost(const nlohmann::json& zones, double px, double py, const nlohmann::json& map);
};

}  // namespace lm
