#include "obstacle_scanner/detection.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <stdexcept>
#include <tuple>

namespace obstacle_scanner
{

std::vector<std::vector<int>> cluster_scan_order(
  const std::vector<Eigen::Vector2d> & points,
  double cluster_gap,
  bool merge_wrap)
{
  std::vector<std::vector<int>> clusters;
  if (points.empty()) {
    return clusters;
  }
  if (cluster_gap <= 0.0) {
    throw std::runtime_error("cluster_gap must be positive.");
  }

  clusters.push_back({0});
  for (std::size_t i = 1; i < points.size(); ++i) {
    const double distance = (points[i] - points[i - 1]).norm();
    if (distance > cluster_gap) {
      clusters.emplace_back();
    }
    clusters.back().push_back(static_cast<int>(i));
  }

  if (merge_wrap && clusters.size() > 1) {
    const int first_index = clusters.front().front();
    const int last_index = clusters.back().back();
    const double wrap_distance = (points[first_index] - points[last_index]).norm();
    if (wrap_distance <= cluster_gap) {
      auto first = clusters.front();
      clusters.back().insert(clusters.back().end(), first.begin(), first.end());
      clusters.erase(clusters.begin());
    }
  }

  return clusters;
}

std::tuple<double, double, double> fit_circle(
  const std::vector<Eigen::Vector2d> & points)
{
  const int count = static_cast<int>(points.size());
  if (count < 3) {
    throw std::runtime_error("Need at least 3 points to fit a circle.");
  }

  Eigen::MatrixXd system(count, 3);
  Eigen::VectorXd target(count);
  for (int i = 0; i < count; ++i) {
    const double x = points[static_cast<std::size_t>(i)].x();
    const double y = points[static_cast<std::size_t>(i)].y();
    system(i, 0) = 2.0 * x;
    system(i, 1) = 2.0 * y;
    system(i, 2) = 1.0;
    target(i) = x * x + y * y;
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
    system, Eigen::ComputeThinU | Eigen::ComputeThinV);
  if (svd.rank() < 3) {
    throw std::runtime_error("Points are nearly collinear.");
  }

  const Eigen::Vector3d solution = svd.solve(target);
  const double center_x = solution(0);
  const double center_y = solution(1);
  const double constant = solution(2);
  const double radius_squared = constant + center_x * center_x + center_y * center_y;
  if (radius_squared <= 0.0 || !std::isfinite(radius_squared)) {
    throw std::runtime_error("Circle fitting produced an invalid radius.");
  }
  return {center_x, center_y, std::sqrt(radius_squared)};
}

std::vector<Observation> detect_observations(
  const std::vector<Eigen::Vector2d> & points,
  const DetectionConfig & config,
  DetectionDebug & debug)
{
  debug = DetectionDebug();
  std::vector<Observation> observations;
  const auto clusters = cluster_scan_order(points, config.cluster_gap, config.merge_wrap);
  debug.total_clusters = static_cast<int>(clusters.size());

  for (const auto & cluster : clusters) {
    const int cluster_size = static_cast<int>(cluster.size());
    if (cluster_size > config.max_cluster_points) {
      debug.discarded_large++;
      continue;
    }

    if (cluster_size == 2) {
      Observation observation;
      observation.type = ObservationType::TwoPointChord;
      observation.p1 = points[static_cast<std::size_t>(cluster[0])];
      observation.p2 = points[static_cast<std::size_t>(cluster[1])];
      observation.cluster_size = cluster_size;
      observations.push_back(observation);
      debug.chord_observations++;
      continue;
    }

    if (cluster_size < 3) {
      debug.skipped_small++;
      continue;
    }

    std::vector<Eigen::Vector2d> cluster_points;
    cluster_points.reserve(static_cast<std::size_t>(cluster_size));
    for (const int index : cluster) {
      cluster_points.push_back(points[static_cast<std::size_t>(index)]);
    }

    try {
      auto [center_x, center_y, radius] = fit_circle(cluster_points);
      if (radius < config.radius_min || radius > config.radius_max) {
        debug.radius_rejected++;
        continue;
      }

      Observation observation;
      observation.type = ObservationType::CircleFit;
      observation.center = Eigen::Vector2d(center_x, center_y);
      observation.radius = radius;
      observation.cluster_size = cluster_size;
      observations.push_back(observation);
      debug.fit_observations++;
    } catch (const std::exception &) {
      debug.fit_failed++;
    }
  }

  return observations;
}

}  // namespace obstacle_scanner
