#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

#include "obstacle_scanner/detection.hpp"
#include "obstacle_scanner/msg/obstacle.hpp"
#include "obstacle_scanner/msg/obstacle_array.hpp"
#include "obstacle_scanner/tracking.hpp"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>

class ObstacleScannerNode : public rclcpp::Node
{
public:
  ObstacleScannerNode()
  : rclcpp::Node("obstacle_scanner")
  {
    using std::placeholders::_1;

    scan_topic_ = declare_parameter<std::string>("scan_topic", "/scan");
    frame_id_ = declare_parameter<std::string>("frame_id", "laser_frame");
    detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1);
    detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20);
    detection_config_.radius_min = declare_parameter("radius_min", 0.02);
    detection_config_.radius_max = declare_parameter("radius_max", 0.05);
    detection_config_.merge_wrap = declare_parameter("merge_wrap", true);

    enable_tracking_ = declare_parameter("enable_tracking", true);
    tracker_config_.track_confirm_fit_hits = declare_parameter("track_confirm_fit_hits", 2);
    tracker_config_.track_confirm_total_hits = declare_parameter("track_confirm_total_hits", 3);
    tracker_config_.track_publish_misses = declare_parameter("track_publish_misses", 3);
    tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 5);
    tracker_config_.process_accel_noise = declare_parameter("process_accel_noise", 3.0);
    tracker_config_.initial_velocity_stddev = declare_parameter("initial_velocity_stddev", 2.5);
    tracker_config_.fit_position_stddev = declare_parameter("fit_position_stddev", 0.02);
    tracker_config_.chord_position_stddev = declare_parameter("chord_position_stddev", 0.06);
    tracker_config_.mahalanobis_gate = declare_parameter("mahalanobis_gate", 9.21);
    tracker_config_.max_association_distance = declare_parameter("max_association_distance", 0.35);
    tracker_config_.max_position_stddev = declare_parameter("max_position_stddev", 0.15);
    tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02);
    tracker_config_.nominal_dt = declare_parameter("nominal_dt", 1.0 / 12.0);
    tracker_config_.min_dt = declare_parameter("min_dt", 0.02);
    tracker_config_.max_dt = declare_parameter("max_dt", 0.20);
    tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_);

    debug_ = declare_parameter("debug", false);
    debug_info_ = declare_parameter("debug_info", true);
    debug_image_size_ = declare_parameter("debug_image_size", 500);
    debug_resolution_ = declare_parameter("debug_resolution", 0.01);
    debug_image_stride_ = std::max(
      1, static_cast<int>(declare_parameter<int>("debug_image_stride", 3)));
    debug_info_stride_ = std::max(
      1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));

    obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
      "/obstacles", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());

    if (debug_) {
      debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
        "/processed_scan", 10);
      if (debug_info_) {
        debug_info_pub_ = create_publisher<std_msgs::msg::String>(
          "/obstacle_scanner/debug_info", 10);
      }
    }

    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
      scan_topic_, rclcpp::SensorDataQoS(),
      std::bind(&ObstacleScannerNode::scan_callback, this, _1));

    RCLCPP_INFO(get_logger(), "ObstacleScannerNode started");
  }

private:
  void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr & msg)
  {
    const auto start_time = std::chrono::steady_clock::now();
    frame_count_++;

    std::vector<Eigen::Vector2d> points;
    points.reserve(msg->ranges.size());

    for (std::size_t i = 0; i < msg->ranges.size(); ++i) {
      const double range = msg->ranges[i];
      if (!std::isfinite(range) || range <= 0.0) {
        continue;
      }
      const double angle = msg->angle_min + static_cast<double>(i) * msg->angle_increment;
      points.emplace_back(range * std::cos(angle), range * std::sin(angle));
    }

    obstacle_scanner::DetectionDebug detection_debug;
    const auto observations = obstacle_scanner::detect_observations(
      points, detection_config_, detection_debug);

    obstacle_scanner::FrameDebug tracker_debug;
    std::vector<obstacle_scanner::TrackOutput> output_tracks;
    std::vector<obstacle_scanner::TrackOutput> debug_tracks;
    if (enable_tracking_) {
      const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
      tracker_debug = tracker_.update(observations, stamp_seconds);
      output_tracks = tracker_.confirmed_tracks();
      debug_tracks = tracker_.tracks_for_debug();
    } else {
      output_tracks = tracks_from_fit_observations(observations);
      debug_tracks = output_tracks;
      tracker_debug.published_tracks = static_cast<int>(output_tracks.size());
      tracker_debug.active_tracks = tracker_debug.published_tracks;
    }

    const double processing_ms = elapsed_ms(start_time);

    RCLCPP_DEBUG(
      get_logger(),
      "points=%zu clusters=%d fit=%d chord=%d tracks=%d published=%zu ms=%.3f",
      points.size(), detection_debug.total_clusters,
      detection_debug.fit_observations, detection_debug.chord_observations,
      tracker_debug.active_tracks, output_tracks.size(), processing_ms);

    auto out_msg = obstacle_scanner::msg::ObstacleArray();
    out_msg.header.stamp = msg->header.stamp;
    out_msg.header.frame_id = frame_id_;
    for (const auto & track : output_tracks) {
      obstacle_scanner::msg::Obstacle obstacle;
      obstacle.center_x = track.center_x;
      obstacle.center_y = track.center_y;
      obstacle.radius = track.radius;
      out_msg.obstacles.push_back(obstacle);
    }
    obstacles_pub_->publish(out_msg);

    if (debug_) {
      if (debug_pub_ && frame_count_ % static_cast<std::uint64_t>(debug_image_stride_) == 0) {
        auto img_msg = render_debug_image(
          points, debug_tracks, detection_debug, tracker_debug,
          processing_ms, msg->header.stamp);
        debug_pub_->publish(*img_msg);
      }
      if (
        debug_info_pub_ &&
        frame_count_ % static_cast<std::uint64_t>(debug_info_stride_) == 0)
      {
        std_msgs::msg::String debug_msg;
        debug_msg.data = build_debug_json(
          points.size(), detection_debug, tracker_debug,
          debug_tracks, output_tracks.size(), processing_ms);
        debug_info_pub_->publish(debug_msg);
      }
    }
  }

  static double elapsed_ms(const std::chrono::steady_clock::time_point & start_time)
  {
    const auto end_time = std::chrono::steady_clock::now();
    return std::chrono::duration<double, std::milli>(end_time - start_time).count();
  }

  static std::vector<obstacle_scanner::TrackOutput> tracks_from_fit_observations(
    const std::vector<obstacle_scanner::Observation> & observations)
  {
    std::vector<obstacle_scanner::TrackOutput> tracks;
    int id = 1;
    for (const auto & observation : observations) {
      if (observation.type != obstacle_scanner::ObservationType::CircleFit) {
        continue;
      }
      obstacle_scanner::TrackOutput track;
      track.id = id++;
      track.center_x = observation.center.x();
      track.center_y = observation.center.y();
      track.radius = observation.radius;
      track.hit_count = 1;
      track.missed_count = 0;
      track.last_source = obstacle_scanner::UpdateSource::Fit;
      tracks.push_back(track);
    }
    return tracks;
  }

  sensor_msgs::msg::Image::SharedPtr render_debug_image(
    const std::vector<Eigen::Vector2d> & points,
    const std::vector<obstacle_scanner::TrackOutput> & tracks,
    const obstacle_scanner::DetectionDebug & detection_debug,
    const obstacle_scanner::FrameDebug & tracker_debug,
    double processing_ms,
    const builtin_interfaces::msg::Time & stamp)
  {
    const int size = debug_image_size_;
    const double res = debug_resolution_;
    const int origin = size / 2;

    cv::Mat img(size, size, CV_8UC3, cv::Scalar(255, 255, 255));

    for (const auto & pt : points) {
      const int col = static_cast<int>(std::round(origin + pt.x() / res));
      const int row = static_cast<int>(std::round(origin - pt.y() / res));
      if (col >= 0 && col < size && row >= 0 && row < size) {
        cv::circle(img, cv::Point(col, row), 1, cv::Scalar(0, 0, 255), cv::FILLED);
      }
    }

    cv::rectangle(
      img, cv::Point(origin - 2, origin - 2),
      cv::Point(origin + 2, origin + 2),
      cv::Scalar(0, 255, 255), cv::FILLED);

    for (const auto & track : tracks) {
      draw_track(img, track, origin, res);
    }

    char summary[160];
    std::snprintf(
      summary, sizeof(summary),
      "pts=%zu cls=%d fit=%d chord=%d trk=%d pub=%d ms=%.1f",
      points.size(), detection_debug.total_clusters,
      detection_debug.fit_observations, detection_debug.chord_observations,
      tracker_debug.active_tracks, tracker_debug.published_tracks, processing_ms);
    cv::putText(
      img, summary, cv::Point(8, size - 12),
      cv::FONT_HERSHEY_SIMPLEX, 0.32, cv::Scalar(40, 40, 40), 1);

    auto image_msg = cv_bridge::CvImage(
      std_msgs::msg::Header(), "bgr8", img).toImageMsg();
    image_msg->header.stamp = stamp;
    image_msg->header.frame_id = frame_id_;
    return image_msg;
  }

  static void draw_track(
    cv::Mat & img,
    const obstacle_scanner::TrackOutput & track,
    int origin,
    double res)
  {
    const int col = static_cast<int>(std::round(origin + track.center_x / res));
    const int row = static_cast<int>(std::round(origin - track.center_y / res));
    const int radius_px = std::max(1, static_cast<int>(std::round(track.radius / res)));

    cv::Scalar color(160, 160, 160);
    int line_type = cv::LINE_8;
    if (track.last_source == obstacle_scanner::UpdateSource::Fit) {
      color = cv::Scalar(0, 180, 0);
    } else if (track.last_source == obstacle_scanner::UpdateSource::Chord) {
      color = cv::Scalar(255, 0, 0);
    } else {
      line_type = cv::LINE_4;
    }

    cv::circle(img, cv::Point(col, row), radius_px, color, 1, line_type);
    cv::circle(img, cv::Point(col, row), 2, color, cv::FILLED);

    char label[48];
    std::snprintf(
      label, sizeof(label), "#%d %s h%d/m%d",
      track.id, obstacle_scanner::update_source_name(track.last_source).c_str(),
      track.hit_count, track.missed_count);
    cv::putText(
      img, label, cv::Point(col + radius_px + 2, row),
      cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1);
  }

  static std::string build_debug_json(
    std::size_t point_count,
    const obstacle_scanner::DetectionDebug & detection_debug,
    const obstacle_scanner::FrameDebug & tracker_debug,
    const std::vector<obstacle_scanner::TrackOutput> & tracks,
    std::size_t published_count,
    double processing_ms)
  {
    std::ostringstream out;
    out << "{";
    out << "\"points\":" << point_count << ",";
    out << "\"clusters\":" << detection_debug.total_clusters << ",";
    out << "\"fit\":" << detection_debug.fit_observations << ",";
    out << "\"chord\":" << detection_debug.chord_observations << ",";
    out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
    out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
    out << "\"tracks_created\":" << tracker_debug.tracks_created << ",";
    out << "\"tracks_confirmed\":" << tracker_debug.tracks_confirmed << ",";
    out << "\"tracks_deleted\":" << tracker_debug.tracks_deleted << ",";
    out << "\"fit_updates\":" << tracker_debug.fit_updates << ",";
    out << "\"association_rejections\":" << tracker_debug.association_rejections << ",";
    out << "\"predicted_published\":" << tracker_debug.predicted_published << ",";
    out << "\"mean_innovation\":" << tracker_debug.mean_innovation << ",";
    out << "\"max_innovation\":" << tracker_debug.max_innovation << ",";
    out << "\"tracks\":" << tracker_debug.active_tracks << ",";
    out << "\"published\":" << published_count << ",";
    out << "\"ms\":" << processing_ms << ",";
    out << "\"sources\":[";
    for (std::size_t i = 0; i < tracks.size(); ++i) {
      if (i > 0) {
        out << ",";
      }
      out << "\"" << obstacle_scanner::update_source_name(tracks[i].last_source) << "\"";
    }
    out << "]}";
    return out.str();
  }

  std::string scan_topic_;
  std::string frame_id_;
  obstacle_scanner::DetectionConfig detection_config_;
  obstacle_scanner::TrackerConfig tracker_config_;
  obstacle_scanner::ObstacleTracker tracker_;
  bool enable_tracking_{true};
  bool debug_{false};
  bool debug_info_{true};
  int debug_image_size_{500};
  double debug_resolution_{0.01};
  int debug_image_stride_{3};
  int debug_info_stride_{1};
  std::uint64_t frame_count_{0};

  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
  rclcpp::Publisher<obstacle_scanner::msg::ObstacleArray>::SharedPtr obstacles_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr debug_info_pub_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ObstacleScannerNode>());
  rclcpp::shutdown();
  return 0;
}
