#pragma once

#include <Eigen/Dense>

#include <limits>
#include <string>
#include <vector>

namespace obstacle_scanner
{

enum class ObservationType
{
  CircleFit,
  TwoPointChord
};

enum class UpdateSource
{
  Fit,
  Chord,
  Predict
};

struct Observation
{
  ObservationType type{ObservationType::CircleFit};
  Eigen::Vector2d center{0.0, 0.0};
  Eigen::Vector2d p1{0.0, 0.0};
  Eigen::Vector2d p2{0.0, 0.0};
  double radius{0.0};
  int cluster_size{0};
};

struct TrackerConfig
{
  int track_confirm_fit_hits{2};
  int track_confirm_total_hits{3};
  int track_publish_misses{3};
  int track_delete_misses{5};
  double process_accel_noise{3.0};
  double initial_velocity_stddev{2.5};
  double fit_position_stddev{0.02};
  double chord_position_stddev{0.06};
  double mahalanobis_gate{9.21};
  double max_association_distance{0.35};
  double max_position_stddev{0.15};
  double chord_tolerance{0.02};
  double nominal_dt{1.0 / 12.0};
  double min_dt{0.02};
  double max_dt{0.20};
};

struct TrackOutput
{
  int id{0};
  double center_x{0.0};
  double center_y{0.0};
  double radius{0.0};
  int hit_count{0};
  int missed_count{0};
  UpdateSource last_source{UpdateSource::Predict};
};

struct FrameDebug
{
  int fit_observations{0};
  int chord_observations{0};
  int chord_updates{0};
  int chord_rejections{0};
  int tracks_created{0};
  int tracks_confirmed{0};
  int tracks_deleted{0};
  int fit_updates{0};
  int association_rejections{0};
  int predicted_published{0};
  double mean_innovation{0.0};
  double max_innovation{0.0};
  int active_tracks{0};
  int published_tracks{0};
};

class ObstacleTracker
{
public:
  explicit ObstacleTracker(const TrackerConfig & config = TrackerConfig());

  FrameDebug update(
    const std::vector<Observation> & observations,
    double stamp_seconds);
  std::vector<TrackOutput> confirmed_tracks() const;
  const std::vector<TrackOutput> & tracks_for_debug() const;

private:
  struct Track
  {
    Eigen::Vector4d state{Eigen::Vector4d::Zero()};
    Eigen::Matrix4d covariance{Eigen::Matrix4d::Zero()};
    double radius{0.0};
    int id{0};
    int fit_hits{0};
    int total_hits{0};
    int missed_count{0};
    bool confirmed{false};
    bool matched{false};
    UpdateSource last_source{UpdateSource::Predict};
  };

  void predict_tracks(double dt);
  void update_track(
    Track & track,
    const Eigen::Vector2d & center,
    double position_stddev,
    UpdateSource source,
    FrameDebug & debug,
    double & innovation_sum,
    int & innovation_count);
  bool chord_center_for_track(
    const Observation & observation,
    const Track & track,
    Eigen::Vector2d & center) const;
  bool is_publishable(const Track & track) const;
  static TrackOutput to_output(const Track & track);
  void refresh_debug_outputs();

  TrackerConfig config_;
  int next_id_{1};
  double last_stamp_seconds_{std::numeric_limits<double>::quiet_NaN()};
  std::vector<Track> tracks_;
  std::vector<TrackOutput> debug_outputs_;
};

std::string update_source_name(UpdateSource source);

}  // namespace obstacle_scanner
