#include <array>
#include <cmath>
#include <functional>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include "origincar_birdseye/birdseye_geometry.hpp"
#include "origincar_birdseye/configuration_validation.hpp"
#include "origincar_birdseye/ground_projection.hpp"
#include "origincar_birdseye/image_validation.hpp"

namespace origincar_birdseye
{

class BirdseyeNode : public rclcpp::Node
{
public:
  BirdseyeNode()
  : Node("birdseye_node"),
    tf_buffer_(this->get_clock()),
    tf_listener_(tf_buffer_)
  {
    const auto image_topic = this->declare_parameter<std::string>("image_topic", "/image");
    const auto image_message_type = this->declare_parameter<std::string>(
      "image_message_type", "raw");
    if (const auto error = validateImageMessageType(image_message_type)) {
      throw std::runtime_error(*error);
    }
    const auto vehicle_output_topic = this->declare_parameter<std::string>(
      "vehicle_output_topic", "/birdseye/vehicle/image");
    const auto world_output_topic = this->declare_parameter<std::string>(
      "world_output_topic", "/birdseye/world/image");
    base_frame_ = this->declare_parameter<std::string>("base_frame", "base_footprint");
    world_frame_ = this->declare_parameter<std::string>("world_frame", "odom_combined");
    enable_world_view_ = this->declare_parameter<bool>("enable_world_view", true);

    const int input_width = this->declare_parameter<int>("input_width", 0);
    const int input_height = this->declare_parameter<int>("input_height", 0);
    input_is_rectified_ = this->declare_parameter<bool>("input_is_rectified", true);
    if (input_width <= 0 || input_height <= 0) {
      throw std::runtime_error("input_width and input_height must be positive YAML parameters");
    }

    const CameraIntrinsics intrinsics{
      this->declare_parameter<double>("fx", 0.0),
      this->declare_parameter<double>("fy", 0.0),
      this->declare_parameter<double>("cx", 0.0),
      this->declare_parameter<double>("cy", 0.0),
      input_width,
      input_height};
    const auto rotation = this->declare_parameter<std::vector<double>>(
      "camera_from_base_rotation", std::vector<double>{});
    const auto translation = this->declare_parameter<std::vector<double>>(
      "camera_from_base_translation", std::vector<double>{});
    if (const auto error = validateCameraConfiguration(intrinsics, rotation, translation)) {
      throw std::runtime_error(*error);
    }

    intrinsics_ = intrinsics;
    camera_from_base_ = RigidTransform(
      {rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
        rotation[7], rotation[8]},
      {translation[0], translation[1], translation[2]});
    projection_ = std::make_unique<GroundProjection>(intrinsics_, camera_from_base_);

    const BirdseyeCanvas vehicle_canvas{
      static_cast<int>(this->declare_parameter<int>("output_width", 600)),
      static_cast<int>(this->declare_parameter<int>("output_height", 800)),
      this->declare_parameter<double>("meters_per_pixel", 0.01),
      this->declare_parameter<double>("rear_meters", 2.0),
      this->declare_parameter<double>("left_meters", 3.0)};
    if (const auto error = validateCanvas(vehicle_canvas, "vehicle")) {
      throw std::runtime_error(*error);
    }
    vehicle_geometry_ = std::make_unique<BirdseyeGeometry>(vehicle_canvas);
    vehicle_canvas_ = vehicle_canvas;

    world_canvas_.width = static_cast<int>(this->declare_parameter<int>("world_width", 800));
    world_canvas_.height = static_cast<int>(this->declare_parameter<int>("world_height", 800));
    world_canvas_.meters_per_pixel = vehicle_canvas.meters_per_pixel;
    world_canvas_.rear_meters = this->declare_parameter<double>("world_rear_meters", 4.0);
    world_canvas_.left_meters = this->declare_parameter<double>("world_left_meters", 4.0);
    world_origin_x_ = this->declare_parameter<double>("world_origin_x", 0.0);
    world_origin_y_ = this->declare_parameter<double>("world_origin_y", 0.0);
    if (const auto error = validateCanvas(world_canvas_, "world")) {
      throw std::runtime_error(*error);
    }
    if (!std::isfinite(world_origin_x_) || !std::isfinite(world_origin_y_)) {
      throw std::runtime_error("world origin must contain finite values");
    }

    const auto distortion = this->declare_parameter<std::vector<double>>(
      "distortion_coefficients", std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0});
    if (const auto error = validateCoefficientVector(
        distortion, 4, "distortion_coefficients"))
    {
      throw std::runtime_error(*error);
    }
    camera_matrix_ = (cv::Mat_<double>(3, 3) << intrinsics.fx, 0.0, intrinsics.cx, 0.0,
      intrinsics.fy, intrinsics.cy, 0.0, 0.0, 1.0);
    distortion_ = cv::Mat(distortion).clone();

    vehicle_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(vehicle_output_topic, 10);
    world_publisher_ = this->create_publisher<sensor_msgs::msg::Image>(world_output_topic, 10);
    if (image_message_type == "compressed") {
      compressed_image_subscription_ =
        this->create_subscription<sensor_msgs::msg::CompressedImage>(
        image_topic, rclcpp::SensorDataQoS(),
        std::bind(&BirdseyeNode::onCompressedImage, this, std::placeholders::_1));
    } else {
      raw_image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
        image_topic, rclcpp::SensorDataQoS(),
        std::bind(&BirdseyeNode::onRawImage, this, std::placeholders::_1));
    }

    RCLCPP_INFO(
      this->get_logger(),
      "birdseye input=%s message_type=%s expected_size=%dx%d rectified=%s "
      "world_frame=%s base_frame=%s",
      image_topic.c_str(), image_message_type.c_str(), intrinsics_.width, intrinsics_.height,
      input_is_rectified_ ? "true" : "false", world_frame_.c_str(), base_frame_.c_str());
  }

private:
  using BaseToCanvas = std::function<std::optional<PixelPoint>(const GroundPoint &)>;

  cv::Mat render(
    const cv::Mat & image, const BaseToCanvas & base_to_canvas, int width,
    int height) const
  {
    const std::array<GroundPoint, 4> reference_points{{
      {0.5, -2.0}, {0.5, 2.0}, {4.0, 2.0}, {4.0, -2.0}}};
    std::vector<cv::Point2f> source;
    std::vector<cv::Point2f> destination;
    source.reserve(reference_points.size());
    destination.reserve(reference_points.size());
    for (const auto & point : reference_points) {
      const auto image_pixel = projection_->projectBaseGroundPoint(point);
      const auto output_pixel = base_to_canvas(point);
      if (!image_pixel || !output_pixel) {
        return cv::Mat::zeros(height, width, CV_8UC3);
      }
      source.emplace_back(image_pixel->x, image_pixel->y);
      destination.emplace_back(output_pixel->x, output_pixel->y);
    }
    cv::Mat output;
    cv::warpPerspective(
      image, output, cv::getPerspectiveTransform(source, destination), cv::Size(width, height),
      cv::INTER_LINEAR, cv::BORDER_CONSTANT);
    return output;
  }

  void drawVehicle(cv::Mat & image, const BaseToCanvas & base_to_canvas) const
  {
    const std::array<GroundPoint, 4> footprint{{
      {0.138, -0.095}, {0.138, 0.095}, {-0.138, 0.095}, {-0.138, -0.095}}};
    std::vector<cv::Point> pixels;
    for (const auto & point : footprint) {
      const auto pixel = base_to_canvas(point);
      if (!pixel) {
        return;
      }
      pixels.emplace_back(static_cast<int>(pixel->x), static_cast<int>(pixel->y));
    }
    cv::fillConvexPoly(image, pixels, cv::Scalar(0, 220, 255));
  }

  std::optional<PixelPoint> worldPixel(const GroundPoint & world_point) const
  {
    const GroundPoint local{world_point.x - world_origin_x_, world_point.y - world_origin_y_};
    return BirdseyeGeometry(world_canvas_).basePointToCanvasPixel(local);
  }

  static GroundPoint baseToWorld(
    const GroundPoint & point, const geometry_msgs::msg::TransformStamped & transform)
  {
    const double yaw = tf2::getYaw(transform.transform.rotation);
    const double cosine = std::cos(yaw);
    const double sine = std::sin(yaw);
    return {
      transform.transform.translation.x + cosine * point.x - sine * point.y,
      transform.transform.translation.y + sine * point.x + cosine * point.y};
  }

  void publish(
    const cv::Mat & image, const std_msgs::msg::Header & header,
    const std::string & frame_id,
    const rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr & publisher) const
  {
    auto message =
      cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, image).toImageMsg();
    message->header.frame_id = frame_id;
    publisher->publish(*message);
  }

  void processImage(const cv::Mat & source, const std_msgs::msg::Header & header)
  {
    const auto size_error = validateImageSize(
      intrinsics_.width, intrinsics_.height,
      source.cols, source.rows);
    if (size_error) {
      RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000, "%s; update input_width/input_height "
        "and matching camera intrinsics in birdseye YAML", size_error->c_str());
      return;
    }

    if (!logged_input_size_) {
      RCLCPP_INFO(this->get_logger(), "accepted input frame size=%dx%d", source.cols, source.rows);
      logged_input_size_ = true;
    }

    cv::Mat projection_input;
    if (input_is_rectified_) {
      projection_input = source;
    } else {
      cv::undistort(source, projection_input, camera_matrix_, distortion_);
    }

    const auto vehicle_mapping = [this](const GroundPoint & point) {
        return vehicle_geometry_->basePointToCanvasPixel(point);
      };
    auto vehicle = render(
      projection_input, vehicle_mapping, vehicle_canvas_.width, vehicle_canvas_.height);
    drawVehicle(vehicle, vehicle_mapping);
    publish(vehicle, header, base_frame_, vehicle_publisher_);

    if (!enable_world_view_) {
      return;
    }
    geometry_msgs::msg::TransformStamped world_from_base;
    try {
      world_from_base = tf_buffer_.lookupTransform(
        world_frame_, base_frame_, header.stamp, tf2::durationFromSec(0.05));
    } catch (const tf2::TransformException & error) {
      RCLCPP_WARN_THROTTLE(
        this->get_logger(),
        *this->get_clock(), 2000, "world view skipped: %s", error.what());
      return;
    }
    const auto world_mapping = [this, &world_from_base](const GroundPoint & point) {
        return worldPixel(baseToWorld(point, world_from_base));
      };
    auto world = render(
      projection_input, world_mapping, world_canvas_.width, world_canvas_.height);
    drawVehicle(world, world_mapping);
    publish(world, header, world_frame_, world_publisher_);
  }

  void onRawImage(const sensor_msgs::msg::Image::SharedPtr message)
  {
    try {
      const auto image = cv_bridge::toCvCopy(message, sensor_msgs::image_encodings::BGR8);
      processImage(image->image, message->header);
    } catch (const cv_bridge::Exception & error) {
      RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000,
        "raw image decode failed: %s", error.what());
    } catch (const cv::Exception & error) {
      RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000,
        "raw image processing failed: %s", error.what());
    }
  }

  void onCompressedImage(const sensor_msgs::msg::CompressedImage::SharedPtr message)
  {
    if (message->data.empty()) {
      RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000,
        "compressed image decode failed: empty data");
      return;
    }
    try {
      const cv::Mat image = cv::imdecode(message->data, cv::IMREAD_COLOR);
      if (image.empty()) {
        RCLCPP_ERROR_THROTTLE(
          this->get_logger(), *this->get_clock(), 2000,
          "compressed image decode failed: empty decoded frame");
        return;
      }
      processImage(image, message->header);
    } catch (const cv::Exception & error) {
      RCLCPP_ERROR_THROTTLE(
        this->get_logger(), *this->get_clock(), 2000,
        "compressed image processing failed: %s", error.what());
    }
  }

  std::string base_frame_;
  std::string world_frame_;
  bool enable_world_view_{true};
  bool input_is_rectified_{true};
  bool logged_input_size_{false};
  CameraIntrinsics intrinsics_{};
  RigidTransform camera_from_base_{{}, {}};
  std::unique_ptr<GroundProjection> projection_;
  BirdseyeCanvas vehicle_canvas_{};
  BirdseyeCanvas world_canvas_{};
  std::unique_ptr<BirdseyeGeometry> vehicle_geometry_;
  double world_origin_x_{0.0};
  double world_origin_y_{0.0};
  cv::Mat camera_matrix_;
  cv::Mat distortion_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr raw_image_subscription_;
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
    compressed_image_subscription_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr vehicle_publisher_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr world_publisher_;
};

}  // namespace origincar_birdseye

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