#include "obstacle_scanner/detection.hpp"

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <vector>

namespace obstacle_scanner
{
namespace
{

DetectionConfig test_config()
{
  DetectionConfig config;
  config.cluster_gap = 0.1;
  config.max_cluster_points = 20;
  config.radius_min = 0.03;
  config.radius_max = 0.05;
  config.merge_wrap = false;
  return config;
}

TEST(DetectionTest, ClustersScanOrderWithoutSorting)
{
  const std::vector<Eigen::Vector2d> points{
    {1.0, 0.0}, {1.02, 0.0}, {1.04, 0.0},
    {1.5, 0.0}, {1.52, 0.0}
  };

  const auto clusters = cluster_scan_order(points, 0.1, false);

  ASSERT_EQ(clusters.size(), 2u);
  EXPECT_EQ(clusters[0].size(), 3u);
  EXPECT_EQ(clusters[1].size(), 2u);
}

TEST(DetectionTest, ThreePointClusterCreatesCircleFitObservation)
{
  auto config = test_config();
  const std::vector<Eigen::Vector2d> points{
    {1.04, 0.0}, {1.0, 0.04}, {0.96, 0.0}
  };

  DetectionDebug debug;
  const auto observations = detect_observations(points, config, debug);

  ASSERT_EQ(observations.size(), 1u);
  EXPECT_EQ(observations[0].type, ObservationType::CircleFit);
  EXPECT_NEAR(observations[0].center.x(), 1.0, 1e-6);
  EXPECT_NEAR(observations[0].center.y(), 0.0, 1e-6);
  EXPECT_NEAR(observations[0].radius, 0.04, 1e-6);
  EXPECT_EQ(debug.fit_observations, 1);
}

TEST(DetectionTest, TwoPointClusterCreatesChordObservation)
{
  auto config = test_config();
  const std::vector<Eigen::Vector2d> points{
    {1.0, -0.02}, {1.0, 0.02}
  };

  DetectionDebug debug;
  const auto observations = detect_observations(points, config, debug);

  ASSERT_EQ(observations.size(), 1u);
  EXPECT_EQ(observations[0].type, ObservationType::TwoPointChord);
  EXPECT_EQ(observations[0].cluster_size, 2);
  EXPECT_EQ(debug.chord_observations, 1);
}

TEST(DetectionTest, LargeClusterIsDiscarded)
{
  auto config = test_config();
  config.max_cluster_points = 3;
  std::vector<Eigen::Vector2d> points;
  for (int i = 0; i < 4; ++i) {
    points.emplace_back(1.0 + 0.01 * static_cast<double>(i), 0.0);
  }

  DetectionDebug debug;
  const auto observations = detect_observations(points, config, debug);

  EXPECT_TRUE(observations.empty());
  EXPECT_EQ(debug.discarded_large, 1);
}

}  // namespace
}  // namespace obstacle_scanner
