#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <std_msgs/msg/header.hpp>
#include <cv_bridge/cv_bridge.hpp>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>


#include <chrono>

using namespace std::chrono_literals;


namespace j7s {
    class ImagePublisher : public rclcpp::Node {
    public:
        ImagePublisher(const rclcpp::NodeOptions& options);
    private:
        std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> m_publisher;
        std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::CompressedImage>> m_compressedPublisher;
        rclcpp::TimerBase::SharedPtr m_timer;

        std::tuple<sensor_msgs::msg::Image::SharedPtr, sensor_msgs::msg::CompressedImage::SharedPtr> draw_image();
    };
};

namespace j7s {
    ImagePublisher::ImagePublisher(const rclcpp::NodeOptions& options):
        Node("image_publisher", options),
        m_publisher(this->create_publisher<sensor_msgs::msg::Image>("image", 1)),
        m_compressedPublisher(this->create_publisher<sensor_msgs::msg::CompressedImage>("image/compressed", 1)),
        m_timer{}
    {
        m_timer = this->create_timer(10ms, [this]() -> void {
            const auto [image, compressedImage] = draw_image();
            m_publisher->publish(*image);
            m_compressedPublisher->publish(*compressedImage);
        });
    }

    std::tuple<sensor_msgs::msg::Image::SharedPtr, sensor_msgs::msg::CompressedImage::SharedPtr> ImagePublisher::draw_image()
    {
        cv::Mat mat = cv::Mat::zeros(200, 200, CV_8UC3);

        std_msgs::msg::Header header;
        header.stamp = this->now();
        cv_bridge::CvImage image(header, "mono8", mat);
        return std::tuple(image.toImageMsg(), image.toCompressedImageMsg());
    }
};

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(j7s::ImagePublisher)