// Video player node for offline debugging

#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>

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

#include <opencv2/opencv.hpp>

#include "rm_utils/heartbeat.hpp"
#include "rm_utils/logger/log.hpp"

namespace fyt::camera_driver {
class VideoPlayerNode : public rclcpp::Node {
public:
  explicit VideoPlayerNode(const rclcpp::NodeOptions &options) : Node("camera_driver", options) {
    FYT_REGISTER_LOGGER("camera_driver", "~/fyt2024-log", INFO);
    FYT_INFO("camera_driver", "Starting VideoPlayerNode!");

    video_path_ = this->declare_parameter("path", "/tmp/video.avi");
    camera_info_url_ =
      this->declare_parameter("camera_info_url", "package://rm_bringup/config/camera_info.yaml");
    frame_id_ = this->declare_parameter("frame_id", "camera_optical_frame");
    frame_rate_ = this->declare_parameter("frame_rate", 30);
    start_frame_ = this->declare_parameter("start_frame", 0);
    keep_looping_ = this->declare_parameter("keep_looping", true);

    heartbeat_ = fyt::HeartBeatPublisher::create(this);

    const std::filesystem::path video_file(video_path_);
    if (!std::filesystem::exists(video_file)) {
      FYT_ERROR("camera_driver", "Video file {} does not exist!", video_file.string());
      rclcpp::shutdown();
      return;
    }

    cap_.open(video_path_);
    if (!cap_.isOpened()) {
      FYT_ERROR("camera_driver", "Video file open failed: {}", video_path_);
      rclcpp::shutdown();
      return;
    }

    image_msg_.header.frame_id = frame_id_;
    image_msg_.encoding = sensor_msgs::image_encodings::BGR8;

    image_msg_.width = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
    image_msg_.height = static_cast<uint32_t>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
    image_msg_.step = image_msg_.width * 3;
    image_msg_.data.resize(static_cast<size_t>(image_msg_.step) * image_msg_.height);

    camera_info_manager_ = std::make_shared<camera_info_manager::CameraInfoManager>(
      this, "camera_driver", "file://" + video_path_);

    if (camera_info_manager_->validateURL(camera_info_url_)) {
      camera_info_manager_->loadCameraInfo(camera_info_url_);
      camera_info_ = camera_info_manager_->getCameraInfo();
    } else {
      camera_info_manager_->setCameraName(video_path_);
      sensor_msgs::msg::CameraInfo camera_info;
      camera_info.header.frame_id = frame_id_;
      camera_info.header.stamp = this->now();
      camera_info.width = image_msg_.width;
      camera_info.height = image_msg_.height;
      camera_info_manager_->setCameraInfo(camera_info);
      FYT_WARN("camera_driver", "Invalid camera info URL: {}", camera_info_url_);
    }

    camera_info_.header.frame_id = frame_id_;
    camera_info_.width = image_msg_.width;
    camera_info_.height = image_msg_.height;

    bool use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
    auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;
    camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);

    const int publish_fps = (frame_rate_ > 0) ? frame_rate_ : 30;
    const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
      std::chrono::duration<double>(1.0 / static_cast<double>(publish_fps)));

    timer_ = this->create_wall_timer(period, std::bind(&VideoPlayerNode::onTimer, this));

    FYT_INFO(
      "camera_driver", "Video player ready: {} ({}x{}, {} fps)", video_path_, image_msg_.width,
      image_msg_.height, publish_fps);
  }

private:
  void onTimer() {
    if (!cap_.isOpened()) {
      return;
    }

    cv::Mat frame;
    cap_ >> frame;

    if (frame.empty()) {
      FYT_INFO("camera_driver", "Video file ends!");
      if (!keep_looping_) {
        rclcpp::shutdown();
        return;
      }

      cap_.release();
      cap_.open(video_path_);
      frame_cnt_ = 0;
      return;
    }

    frame_cnt_++;
    if (frame_cnt_ < start_frame_) {
      return;
    }

    if (frame.cols != static_cast<int>(image_msg_.width) ||
        frame.rows != static_cast<int>(image_msg_.height) || frame.type() != CV_8UC3) {
      cv::Mat bgr;
      if (frame.channels() == 3) {
        bgr = frame;
      } else if (frame.channels() == 1) {
        cv::cvtColor(frame, bgr, cv::COLOR_GRAY2BGR);
      } else {
        frame.convertTo(bgr, CV_8UC3);
      }
      if (bgr.cols != static_cast<int>(image_msg_.width) || bgr.rows != static_cast<int>(image_msg_.height)) {
        cv::resize(bgr, bgr, cv::Size(static_cast<int>(image_msg_.width), static_cast<int>(image_msg_.height)));
      }
      std::memcpy(image_msg_.data.data(), bgr.data, image_msg_.data.size());
    } else {
      std::memcpy(image_msg_.data.data(), frame.data, image_msg_.data.size());
    }

    auto stamp = this->now();
    image_msg_.header.stamp = stamp;
    camera_info_.header.stamp = stamp;
    camera_info_.header.frame_id = frame_id_;
    image_msg_.header.frame_id = frame_id_;

    camera_pub_.publish(image_msg_, camera_info_);
  }

  fyt::HeartBeatPublisher::SharedPtr heartbeat_;

  std::string video_path_;
  std::string camera_info_url_;
  std::string frame_id_;
  int frame_rate_{30};
  int start_frame_{0};
  bool keep_looping_{true};
  int frame_cnt_{0};

  cv::VideoCapture cap_;

  image_transport::CameraPublisher camera_pub_;
  sensor_msgs::msg::Image image_msg_;
  sensor_msgs::msg::CameraInfo camera_info_;
  std::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;

  rclcpp::TimerBase::SharedPtr timer_;
};
}  // namespace fyt::camera_driver

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(fyt::camera_driver::VideoPlayerNode)
