#pragma once

#include "obstacle_scanner/tracking.hpp"

#include <Eigen/Dense>

#include <vector>

namespace obstacle_scanner
{

struct DetectionConfig
{
  double cluster_gap{0.1};
  int max_cluster_points{20};
  double radius_min{0.03};
  double radius_max{0.05};
  bool merge_wrap{true};
};

struct DetectionDebug
{
  int total_clusters{0};
  int fit_observations{0};
  int chord_observations{0};
  int discarded_large{0};
  int skipped_small{0};
  int fit_failed{0};
  int radius_rejected{0};
};

std::vector<std::vector<int>> cluster_scan_order(
  const std::vector<Eigen::Vector2d> & points,
  double cluster_gap,
  bool merge_wrap);

std::tuple<double, double, double> fit_circle(
  const std::vector<Eigen::Vector2d> & points);

std::vector<Observation> detect_observations(
  const std::vector<Eigen::Vector2d> & points,
  const DetectionConfig & config,
  DetectionDebug & debug);

}  // namespace obstacle_scanner
