#include "obstacle_scanner/tracking.hpp"

#include <gtest/gtest.h>

#include <cmath>
#include <limits>
#include <stdexcept>

namespace obstacle_scanner
{
namespace
{

TrackerConfig test_config()
{
  TrackerConfig config;
  config.track_confirm_fit_hits = 2;
  config.track_confirm_total_hits = 3;
  config.track_publish_misses = 3;
  config.track_delete_misses = 5;
  config.process_accel_noise = 3.0;
  config.initial_velocity_stddev = 2.5;
  config.fit_position_stddev = 0.02;
  config.chord_position_stddev = 0.06;
  config.mahalanobis_gate = 9.21;
  config.max_association_distance = 0.35;
  config.max_position_stddev = 0.15;
  config.chord_tolerance = 0.02;
  config.nominal_dt = 1.0 / 12.0;
  config.min_dt = 0.02;
  config.max_dt = 0.20;
  return config;
}

Observation fit(double x, double y = 0.0, double radius = 0.04)
{
  Observation observation;
  observation.type = ObservationType::CircleFit;
  observation.center = Eigen::Vector2d(x, y);
  observation.radius = radius;
  observation.cluster_size = 4;
  return observation;
}

Observation chord(double x, double half_height = 0.02)
{
  Observation observation;
  observation.type = ObservationType::TwoPointChord;
  observation.p1 = Eigen::Vector2d(x, -half_height);
  observation.p2 = Eigen::Vector2d(x, half_height);
  observation.cluster_size = 2;
  return observation;
}

void confirm_track(ObstacleTracker & tracker, double x = 1.0)
{
  tracker.update({fit(x)}, 0.0);
  tracker.update({fit(x)}, 1.0 / 12.0);
}

void seed_moving_track(ObstacleTracker & tracker)
{
  tracker.update({fit(1.0)}, 0.0);
  tracker.update({fit(1.1666667)}, 1.0 / 12.0);
  tracker.update({fit(1.3333333)}, 2.0 / 12.0);
}

TEST(ObstacleTrackerTest, FirstFitIsTentativeAndSecondFitConfirms)
{
  ObstacleTracker tracker(test_config());

  const FrameDebug first = tracker.update({fit(1.0, 0.2)}, 0.0);
  EXPECT_TRUE(tracker.confirmed_tracks().empty());
  EXPECT_EQ(first.tracks_created, 1);
  EXPECT_EQ(first.tracks_confirmed, 0);
  EXPECT_EQ(first.fit_updates, 1);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 1);

  const FrameDebug second = tracker.update({fit(1.0, 0.2)}, 1.0 / 12.0);
  const auto tracks = tracker.confirmed_tracks();
  ASSERT_EQ(tracks.size(), 1u);
  EXPECT_EQ(second.tracks_confirmed, 1);
  EXPECT_EQ(second.fit_updates, 1);
  EXPECT_EQ(tracks[0].hit_count, 2);
  EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit);
  EXPECT_NEAR(tracks[0].center_x, 1.0, 1e-9);
  EXPECT_NEAR(tracks[0].center_y, 0.2, 1e-9);
}

TEST(ObstacleTrackerTest, TracksTwoMetersPerSecondAcrossOneEmptyFrame)
{
  ObstacleTracker tracker(test_config());
  tracker.update({fit(1.0)}, 0.0);
  tracker.update({fit(1.1666667)}, 1.0 / 12.0);
  tracker.update({fit(1.3333333)}, 2.0 / 12.0);

  const FrameDebug debug = tracker.update({}, 3.0 / 12.0);
  const auto tracks = tracker.confirmed_tracks();
  ASSERT_EQ(tracks.size(), 1u);
  EXPECT_EQ(debug.predicted_published, 1);
  EXPECT_EQ(tracks[0].last_source, UpdateSource::Predict);
  EXPECT_NEAR(tracks[0].center_x, 1.5, 0.08);
}

TEST(ObstacleTrackerTest, TwoFitsSeparatedBySixteenCentimetersShareOneTrack)
{
  ObstacleTracker tracker(test_config());

  tracker.update({fit(1.0)}, 0.0);
  const FrameDebug debug = tracker.update({fit(1.1666667)}, 1.0 / 12.0);

  EXPECT_EQ(debug.tracks_created, 0);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
}

TEST(ObstacleTrackerTest, FarFitStartsTentativeTrackWithoutDuplicatePublication)
{
  ObstacleTracker tracker(test_config());
  confirm_track(tracker);

  const FrameDebug debug = tracker.update({fit(1.5)}, 2.0 / 12.0);
  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
  EXPECT_EQ(debug.tracks_created, 1);
  EXPECT_EQ(debug.association_rejections, 1);
}

TEST(ObstacleTrackerTest, OneFitAndTwoChordsConfirmOneTrack)
{
  ObstacleTracker tracker(test_config());

  tracker.update({fit(1.0)}, 0.0);
  const FrameDebug first_chord = tracker.update({chord(0.985)}, 1.0 / 12.0);
  const FrameDebug second_chord = tracker.update({chord(0.985)}, 2.0 / 12.0);

  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
  EXPECT_EQ(first_chord.chord_updates, 1);
  EXPECT_EQ(second_chord.chord_updates, 1);
  EXPECT_EQ(second_chord.tracks_confirmed, 1);
}

TEST(ObstacleTrackerTest, ChordBeyondAssociationDistanceIsRejected)
{
  ObstacleTracker tracker(test_config());
  confirm_track(tracker);

  const FrameDebug debug = tracker.update({chord(1.5, 0.01)}, 2.0 / 12.0);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
  EXPECT_EQ(debug.chord_updates, 0);
  EXPECT_EQ(debug.chord_rejections, 0);
  EXPECT_EQ(debug.association_rejections, 1);
}

TEST(ObstacleTrackerTest, ValidChordUsesFarSideCandidate)
{
  ObstacleTracker tracker(test_config());
  tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);

  const FrameDebug debug = tracker.update({chord(0.985)}, 1.0 / 12.0);
  ASSERT_EQ(debug.chord_updates, 1);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
  EXPECT_GT(tracker.tracks_for_debug()[0].center_x, 1.0);
}

TEST(ObstacleTrackerTest, ConfirmedTrackPublishesThroughThirdMissOnly)
{
  ObstacleTracker tracker(test_config());
  confirm_track(tracker);

  EXPECT_EQ(tracker.update({}, 2.0 / 12.0).published_tracks, 1);
  EXPECT_EQ(tracker.update({}, 3.0 / 12.0).published_tracks, 1);
  EXPECT_EQ(tracker.update({}, 4.0 / 12.0).published_tracks, 1);
  EXPECT_EQ(tracker.update({}, 5.0 / 12.0).published_tracks, 0);
  EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
  EXPECT_EQ(tracker.update({}, 6.0 / 12.0).tracks_deleted, 1);
  EXPECT_TRUE(tracker.tracks_for_debug().empty());
}

TEST(ObstacleTrackerTest, LargePositionUncertaintyStopsPublication)
{
  TrackerConfig config = test_config();
  config.process_accel_noise = 10.0;
  config.max_position_stddev = 0.02;
  ObstacleTracker tracker(config);
  confirm_track(tracker);

  const FrameDebug debug = tracker.update({}, 2.0 / 12.0);
  EXPECT_EQ(debug.published_tracks, 0);
  EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
}

TEST(ObstacleTrackerTest, TentativeTrackIsDeletedOnFirstMiss)
{
  ObstacleTracker tracker(test_config());
  tracker.update({fit(1.0)}, 0.0);

  const FrameDebug debug = tracker.update({}, 1.0 / 12.0);
  EXPECT_EQ(debug.tracks_deleted, 1);
  EXPECT_TRUE(tracker.tracks_for_debug().empty());
}

TEST(ObstacleTrackerTest, InvalidTimestampBoundsAreRejected)
{
  TrackerConfig config = test_config();
  config.min_dt = 0.20;
  config.max_dt = 0.02;

  EXPECT_THROW(ObstacleTracker tracker(config), std::invalid_argument);
}

TEST(ObstacleTrackerTest, DuplicateAndInvalidTimestampsUseNominalDelta)
{
  ObstacleTracker reference(test_config());
  ObstacleTracker duplicate(test_config());
  ObstacleTracker invalid(test_config());
  seed_moving_track(reference);
  seed_moving_track(duplicate);
  seed_moving_track(invalid);

  reference.update({}, 3.0 / 12.0);
  duplicate.update({}, 2.0 / 12.0);
  invalid.update({}, std::numeric_limits<double>::quiet_NaN());

  ASSERT_EQ(reference.tracks_for_debug().size(), 1u);
  ASSERT_EQ(duplicate.tracks_for_debug().size(), 1u);
  ASSERT_EQ(invalid.tracks_for_debug().size(), 1u);
  EXPECT_NEAR(
    duplicate.tracks_for_debug()[0].center_x,
    reference.tracks_for_debug()[0].center_x, 1e-9);
  EXPECT_NEAR(
    invalid.tracks_for_debug()[0].center_x,
    reference.tracks_for_debug()[0].center_x, 1e-9);
}

TEST(ObstacleTrackerTest, ValidTimestampDeltaIsClampedToConfiguredBounds)
{
  ObstacleTracker lower_reference(test_config());
  ObstacleTracker lower_clamped(test_config());
  ObstacleTracker upper_reference(test_config());
  ObstacleTracker upper_clamped(test_config());
  seed_moving_track(lower_reference);
  seed_moving_track(lower_clamped);
  seed_moving_track(upper_reference);
  seed_moving_track(upper_clamped);
  const double last_stamp = 2.0 / 12.0;

  lower_reference.update({}, last_stamp + test_config().min_dt);
  lower_clamped.update({}, last_stamp + 0.001);
  upper_reference.update({}, last_stamp + test_config().max_dt);
  upper_clamped.update({}, last_stamp + 1.0);

  EXPECT_NEAR(
    lower_clamped.tracks_for_debug()[0].center_x,
    lower_reference.tracks_for_debug()[0].center_x, 1e-9);
  EXPECT_NEAR(
    upper_clamped.tracks_for_debug()[0].center_x,
    upper_reference.tracks_for_debug()[0].center_x, 1e-9);
}

TEST(ObstacleTrackerTest, GreedyAssociationAssignsTwoFitsToTwoTracks)
{
  ObstacleTracker tracker(test_config());
  tracker.update({fit(1.0), fit(2.0)}, 0.0);

  const FrameDebug debug = tracker.update(
    {fit(2.05), fit(1.05)}, 1.0 / 12.0);

  EXPECT_EQ(debug.tracks_created, 0);
  EXPECT_EQ(debug.fit_updates, 2);
  ASSERT_EQ(tracker.confirmed_tracks().size(), 2u);
  ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
  EXPECT_EQ(tracker.tracks_for_debug()[1].hit_count, 2);
}

TEST(ObstacleTrackerTest, FitAssociationTakesPriorityOverChordInSameFrame)
{
  ObstacleTracker tracker(test_config());
  tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);

  const FrameDebug debug = tracker.update(
    {chord(0.985), fit(1.01, 0.0, 0.05)}, 1.0 / 12.0);

  EXPECT_EQ(debug.fit_updates, 1);
  EXPECT_EQ(debug.chord_updates, 0);
  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
  EXPECT_EQ(tracker.confirmed_tracks()[0].last_source, UpdateSource::Fit);
}

}  // namespace
}  // namespace obstacle_scanner
