#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>

#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_storage/storage_options.hpp>

#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>

#include <string>
#include <memory>
#include <chrono>

class VideoToRosbag : public rclcpp::Node
{
public:
  VideoToRosbag()
      : Node("video_to_rosbag")
  {
    // Parameters
    this->declare_parameter<std::string>("input_video", "input.mp4");
    this->declare_parameter<std::string>("output_bag", "output_bag");
    this->declare_parameter<std::string>("topic", "/camera/image_raw");
    this->declare_parameter<std::string>("storage_id", "mcap"); // mcap is default on Jazzy [web:32]
    this->declare_parameter<std::string>("frame_id", "camera_frame");
    this->declare_parameter<double>("fps_override", 0.0);

    const std::string input_video = this->get_parameter("input_video").as_string();
    const std::string output_bag = this->get_parameter("output_bag").as_string();
    const std::string topic = this->get_parameter("topic").as_string();
    const std::string storage_id = this->get_parameter("storage_id").as_string();
    const std::string frame_id = this->get_parameter("frame_id").as_string();
    double fps_override = this->get_parameter("fps_override").as_double();

    RCLCPP_INFO(get_logger(), "Input video: %s", input_video.c_str());
    RCLCPP_INFO(get_logger(), "Output bag:  %s (storage_id=%s)", output_bag.c_str(), storage_id.c_str());
    RCLCPP_INFO(get_logger(), "Topic:       %s", topic.c_str());

    // Open video
    cv::VideoCapture cap(input_video);
    if (!cap.isOpened())
    {
      RCLCPP_ERROR(get_logger(), "Failed to open video file: %s", input_video.c_str());
      rclcpp::shutdown();
      return;
    }

    // Video properties
    double video_fps = cap.get(cv::CAP_PROP_FPS);
    int total_frames = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_COUNT));
    int width = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_WIDTH));
    int height = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_HEIGHT));

    double fps;
    if (fps_override > 0.0)
    {
      fps = fps_override;
      RCLCPP_WARN(get_logger(), "Using FPS override: %.3f (video reports %.3f)", fps, video_fps);
    }
    else if (video_fps > 0.0)
    {
      fps = video_fps;
      RCLCPP_INFO(get_logger(), "Detected FPS from video: %.3f", fps);
    }
    else
    {
      fps = 30.0;
      RCLCPP_WARN(get_logger(), "Video FPS unknown, falling back to %.3f", fps);
    }

    RCLCPP_INFO(get_logger(), "Video size: %dx%d, frames: %d, fps used: %.3f",
                width, height, total_frames, fps);

    // rosbag2 storage options (FIXED line: assign string, not struct)
    rosbag2_storage::StorageOptions storage_options;
    storage_options.uri = output_bag;
    storage_options.storage_id = storage_id; // <- was 'storage_options' before (compile error)

    rosbag2_cpp::ConverterOptions converter_options;
    converter_options.input_serialization_format = rmw_get_serialization_format();
    converter_options.output_serialization_format = rmw_get_serialization_format();
    auto writer = std::make_unique<rosbag2_cpp::Writer>();
    writer->open(storage_options, converter_options); // Jazzy API, mirrors tutorial [web:32]

    // Time base for frame stamps
    rclcpp::Time start_time = this->now();

    cv::Mat frame;
    int frame_count = 0;

    RCLCPP_INFO(get_logger(), "Starting frame loop...");

    while (cap.read(frame))
    {
      if (frame.empty())
      {
        break;
      }

      sensor_msgs::msg::Image msg;

      // Timestamp based on frame index and FPS
      rclcpp::Time stamp = start_time +
                           rclcpp::Duration::from_seconds(static_cast<double>(frame_count) / fps);

      msg.header.stamp = stamp;
      msg.header.frame_id = frame_id;
      msg.height = static_cast<uint32_t>(frame.rows);
      msg.width = static_cast<uint32_t>(frame.cols);
      msg.encoding = "bgr8"; // OpenCV default color layout
      msg.is_bigendian = false;
      msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);

      const size_t size_in_bytes = static_cast<size_t>(frame.step) * frame.rows;
      msg.data.assign(frame.data, frame.data + size_in_bytes);

      // Let rosbag2_cpp::Writer serialize and auto‑create the topic [web:29][web:32]
      writer->write(msg, topic, stamp);

      ++frame_count;
      if (total_frames > 0 && frame_count % 100 == 0)
      {
        double progress = 100.0 * static_cast<double>(frame_count) /
                          static_cast<double>(total_frames);
        RCLCPP_INFO(get_logger(), "Frames: %d / %d (%.1f%%)",
                    frame_count, total_frames, progress);
      }
    }

    cap.release();
    writer->close();

    RCLCPP_INFO(get_logger(), "Done. Wrote %d frames to bag '%s'", frame_count, output_bag.c_str());
  }
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<VideoToRosbag>();
  // All work is done in the constructor; we don't need to spin.
  rclcpp::shutdown();
  return 0;
}