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

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

namespace origincar_birdseye
{

class SyntheticCameraNode : public rclcpp::Node
{
public:
  SyntheticCameraNode()
  : Node("synthetic_camera_node")
  {
    const auto image_topic = this->declare_parameter<std::string>("image_topic", "/image");
    width_ = this->declare_parameter<int>("input_width", 0);
    height_ = this->declare_parameter<int>("input_height", 0);
    if (width_ <= 0 || height_ <= 0) {
      throw std::runtime_error("synthetic input_width and input_height must be positive");
    }
    publisher_ = this->create_publisher<sensor_msgs::msg::Image>(image_topic, 10);
    timer_ = this->create_wall_timer(std::chrono::milliseconds(100), [this]() {publishFrame();});
  }

private:
  void publishFrame()
  {
    const auto width = static_cast<uint32_t>(width_);
    const auto height = static_cast<uint32_t>(height_);
    sensor_msgs::msg::Image message;
    message.header.stamp = this->now();
    message.header.frame_id = "front_camera_optical_frame";
    message.height = height;
    message.width = width;
    message.encoding = sensor_msgs::image_encodings::RGB8;
    message.step = width * 3;
    message.data.resize(static_cast<size_t>(message.step) * height);
    for (uint32_t row = 0; row < height; ++row) {
      for (uint32_t column = 0; column < width; ++column) {
        const bool line = row % 40 < 2 || column % 40 < 2;
        const size_t index = static_cast<size_t>(row) * message.step + column * 3;
        message.data[index] = line ? 240 : 50;
        message.data[index + 1] = line ? 240 : 100;
        message.data[index + 2] = line ? 240 : 50;
      }
    }
    publisher_->publish(message);
  }

  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  int width_{0};
  int height_{0};
};

}  // namespace origincar_birdseye

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