/*
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
   http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include "cloud_reprojection_ros_node.hpp"

#include <algorithm>
#include <fstream>
#include <sys/stat.h>
#include <thread>
#include <chrono>
#include <vector>
#include <cstdint>

#ifdef ROS2
#include <sensor_msgs/msg/compressed_image.hpp>
#endif

namespace {

cv::Mat resizeToHeight(const cv::Mat& src, int target_h)
{
    if (src.empty() || target_h <= 0) {
        return src.clone();
    }
    if (src.rows == target_h) {
        return src.clone();
    }
    const double scale = static_cast<double>(target_h) / static_cast<double>(src.rows);
    cv::Mat out;
    cv::resize(src, out, cv::Size(), scale, scale, cv::INTER_LINEAR);
    return out;
}

/** Decode JPEG/PNG payload from sensor_msgs/CompressedImage to BGR. */
cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len)
{
    if (!data || len == 0) {
        return cv::Mat();
    }
    cv::Mat raw(1, static_cast<int>(len), CV_8UC1, const_cast<uint8_t*>(data));
    return cv::imdecode(raw, cv::IMREAD_COLOR);
}

cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr,
                                     const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
                                     const CloudReprojector::CameraParams& cam_params,
                                     int point_radius = 2,
                                     float min_depth = 0.5f,
                                     float max_depth = 30.0f)
{
    if (camera_bgr.empty()) {
        return cv::Mat();
    }

    cv::Mat overlay = camera_bgr.clone();
    mini_vikit::PolynomialCamera camera_model(
        cam_params.image_width, cam_params.image_height,
        cam_params.A11, cam_params.A22,
        cam_params.u0, cam_params.v0,
        cam_params.A12,
        cam_params.k2, cam_params.k3, cam_params.k4,
        cam_params.k5, cam_params.k6, cam_params.k7);

    std::vector<cv::Point> pixels;
    std::vector<float> depths;
    pixels.reserve(cloud_in_cam.size());
    depths.reserve(cloud_in_cam.size());

    for (const auto& pt : cloud_in_cam) {
        if (pt.z <= min_depth || pt.z >= max_depth) {
            continue;
        }

        const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
        const int u = static_cast<int>(std::round(uv.x()));
        const int v = static_cast<int>(std::round(uv.y()));
        if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
            continue;
        }

        pixels.emplace_back(u, v);
        depths.emplace_back(static_cast<float>(pt.z));
    }

    if (pixels.empty()) {
        return overlay;
    }

    cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
    cv::Mat clipped, norm_u8, colors;
    cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
    clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
    clipped.convertTo(norm_u8, CV_8U);
    cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);

    for (int i = 0; i < colors.rows; ++i) {
        const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
        cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
                   cv::Scalar(color[0], color[1], color[2]), -1);
    }

    return overlay;
}

}  // namespace

#ifdef ROS2
    #include <functional>
    #include <yaml-cpp/yaml.h>
    #include <rcpputils/filesystem_helper.hpp>
#else
    #include <boost/bind.hpp>
#endif

// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu
// TODO: Fill in the actual Til values for your sensor setup
static Eigen::Matrix4d getFixedTil()
{
    Eigen::Matrix4d Til = Eigen::Matrix4d::Identity();
    Til(0, 3) = -0.02663;
    Til(1, 3) = 0.03447;
    Til(2, 3) = 0.02174;
    return Til;
}

static bool fileExists(const std::string& filename) {
    struct stat buffer;
    return (stat(filename.c_str(), &buffer) == 0);
}

#ifdef ROS2
// Helper function to get package source directory for ROS2
static std::string get_package_source_directory() {
    std::string current_file = __FILE__;
    size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp");
    if (pos != std::string::npos) {
        return current_file.substr(0, pos);
    }
    return "";
}

// ==================== ROS2 Implementation ====================

CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options)
    : Node("cloud_reprojection_node", options)
{
    loadParameters();

    RCLCPP_INFO_STREAM(this->get_logger(),
        "\n  cloud_slam_topic: " << cloud_slam_topic_
        << "\n  odometry_topic: " << odometry_topic_
        << "\n  wiwc_topic: " << wiwc_topic_
        << "\n  camera_image_topic: " << camera_image_topic_
        << "\n  sync_* topics under: " << sync_topic_prefix_
        << "\n  overlay_compressed_topic: " << sync_overlay_image_topic_
        << "\n  combined_compressed_topic: " << combined_compressed_topic_
        << "\n  publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
        << "\n  send_overlay: " << (send_overlay_ ? "on" : "off"));

    cloud_sub_.subscribe(this, cloud_slam_topic_);
    odom_sub_.subscribe(this, odometry_topic_);
    wiwc_sub_.subscribe(this, wiwc_topic_);
    image_compressed_sub_.subscribe(this, camera_image_topic_);

    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_);
    sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
                                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                                       std::placeholders::_4));

    sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
    sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
    sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
    sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
    sync_image_pub_ = this->create_publisher<CompressedImage>(sync_image_topic_, 10);

    if (send_overlay_) {
        overlay_compressed_pub_ =
            this->create_publisher<CompressedImage>(sync_overlay_image_topic_, 10);
    }
    if (publish_combined_compressed_) {
        combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
    }

    RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
}

void CloudReprojectionRosNode::loadParameters()
{
    // Declare and get parameters
    this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
    this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
    this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
    this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image/compressed");
    this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
    this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
    this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
    this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
    this->declare_parameter<int>("register_keys.send_overlay", 1);
    this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);

    cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
    odometry_topic_ = this->get_parameter("odometry_topic").as_string();
    wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
    camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
    sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
    combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
    combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
    combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_));
    overlay_jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int();
    overlay_jpeg_quality_ = std::max(1, std::min(100, overlay_jpeg_quality_));
    publish_combined_compressed_ =
        (this->get_parameter("register_keys.send_combined_compressed").as_int() != 0);
    send_overlay_ = (this->get_parameter("register_keys.send_overlay").as_int() != 0);

    sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
    sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
    sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
    sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
    sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
    sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";

    // Load camera parameters from calib.yaml file directly
    std::string package_path = get_package_source_directory();
    std::string calib_file = package_path + "/config/calib.yaml";
    
    YAML::Node calib_config;
    try {
        calib_config = YAML::LoadFile(calib_file);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what());
        rclcpp::shutdown();
        return;
    }

    CloudReprojector::CameraParams cam_params;
    try {
        cam_params.image_width = calib_config["cam_0"]["image_width"].as<int>();
        cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
        cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
        cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
        cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
        cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
        cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
        cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
        cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
        cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
        cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
        cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
        cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what());
        rclcpp::shutdown();
        return;
    }

    // Load extrinsic parameters
    CloudReprojector::ExtrinsicParams ext_params;
    try {
        auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
        
        if (Tcl_vec.size() == 16)
        {
            for (int i = 0; i < 4; ++i)
                for (int j = 0; j < 4; ++j)
                    ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j];
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size());
            rclcpp::shutdown();
            return;
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what());
        rclcpp::shutdown();
        return;
    }

    ext_params.Til = getFixedTil();
    ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);

    RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
    RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til);
    RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic);

    RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
    RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height);
    RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
                cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);

    reprojector_ = std::make_unique<CloudReprojector>();
    if (!reprojector_->initialize(cam_params, ext_params))
    {
        RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
        rclcpp::shutdown();
    }
}

void CloudReprojectionRosNode::syncCallback(
    const PointCloud2::ConstSharedPtr& cloud_msg,
    const Odometry::ConstSharedPtr& odom_msg,
    const Odometry::ConstSharedPtr& wiwc_msg,
    const CompressedImage::ConstSharedPtr& image_compressed_msg)
{
    const auto sync_stamp = image_compressed_msg->header.stamp;

    PointCloud2 sync_cloud_slam_msg = *cloud_msg;
    sync_cloud_slam_msg.header.stamp = sync_stamp;
    Odometry sync_odom_msg = *odom_msg;
    sync_odom_msg.header.stamp = sync_stamp;
    Odometry sync_wiwc_msg = *wiwc_msg;
    sync_wiwc_msg.header.stamp = sync_stamp;
    CompressedImage sync_image_msg = *image_compressed_msg;
    sync_image_msg.header.stamp = sync_stamp;



    pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
    pcl::fromROSMsg(*cloud_msg, cloud_odom);

    if (cloud_odom.empty())
    {
        RCLCPP_WARN(this->get_logger(), "Empty cloud_slam received");
        return;
    }

    Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
    for (int i = 0; i < 16; ++i) {
        T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
        T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
    }

    bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
    bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;

    if (T_CL_valid && T_IL_valid) {
        reprojector_->updateExtrinsics(T_CL, T_IL);
    }

    CloudReprojector::OdomPose odom_pose;
    odom_pose.orientation = Eigen::Quaterniond(
        odom_msg->pose.pose.orientation.w,
        odom_msg->pose.pose.orientation.x,
        odom_msg->pose.pose.orientation.y,
        odom_msg->pose.pose.orientation.z
    );
    odom_pose.position = Eigen::Vector3d(
        odom_msg->pose.pose.position.x,
        odom_msg->pose.pose.position.y,
        odom_msg->pose.pose.position.z
    );

    pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
        reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
    if (cloud_cam.empty())
    {
        RCLCPP_WARN(this->get_logger(), "Camera-frame cloud is empty after reprojection");
        return;
    }

    PointCloud2 cloud_cam_msg;
    pcl::toROSMsg(cloud_cam, cloud_cam_msg);
    cloud_cam_msg.header = image_compressed_msg->header;
    cloud_cam_msg.header.stamp = sync_stamp;
    if (cloud_cam_msg.header.frame_id.empty()) {
        cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
    }


    const bool need_combined = (combined_pub_ != nullptr);
    cv::Mat depth_vis;
    if (need_combined) {
        depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
        if (depth_vis.empty()) {
            return;
        }
    }

    cv::Mat cam_bgr;
    if (send_overlay_ || need_combined) {
        cam_bgr = decodeCompressedToBgr(
            image_compressed_msg->data.data(), image_compressed_msg->data.size());
        if (cam_bgr.empty()) {
            RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
                         image_compressed_msg->format.c_str());
            return;
        }
    }

    sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
    sync_odom_pub_->publish(sync_odom_msg);
    sync_wiwc_pub_->publish(sync_wiwc_msg);
    sync_cloud_pub_->publish(cloud_cam_msg);
    sync_image_pub_->publish(sync_image_msg);

    if (send_overlay_ && overlay_compressed_pub_) {
        cv::Mat overlay_vis = overlayProjectedCloudOnImage(
            cam_bgr, cloud_cam, reprojector_->getCameraParams());
        if (!overlay_vis.empty()) {
            std::vector<uchar> obuf;
            const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
            if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
                RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)");
                return;
            }
            CompressedImage omsg;
            omsg.header = image_compressed_msg->header;
            omsg.format = "jpeg";
            omsg.data.assign(obuf.begin(), obuf.end());
            overlay_compressed_pub_->publish(omsg);
        }
    }

    if (combined_pub_) {
        const int H = std::max(depth_vis.rows, cam_bgr.rows);
        cv::Mat left = resizeToHeight(depth_vis, H);
        cv::Mat right = resizeToHeight(cam_bgr, H);
        cv::Mat combined;
        cv::hconcat(left, right, combined);

        std::vector<uchar> buf;
        const std::vector<int> enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_};
        if (!cv::imencode(".jpg", combined, buf, enc_params)) {
            RCLCPP_ERROR(this->get_logger(), "cv::imencode failed");
            return;
        }

        CompressedImage out;
        out.header = image_compressed_msg->header;
        out.format = "jpeg";
        out.data.assign(buf.begin(), buf.end());
        combined_pub_->publish(out);
    }
}

// ==================== ROS2 Main ====================
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    auto temp_node = std::make_shared<rclcpp::Node>("cloud_reprojection_check");
    
    // Check if reprojection is enabled from control_command.yaml
    std::string package_path = get_package_source_directory();
    std::string config_file = package_path + "/config/control_command.yaml";
    
    try {
        YAML::Node config = YAML::LoadFile(config_file);
        std::cout << "config: " << config_file << std::endl;
        if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) {
            RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled.");
            rclcpp::shutdown();
            return 0;
        }
        
        int sendreprojection = config["register_keys"]["sendreprojection"].as<int>();
        if (sendreprojection == 0) {
            RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published.");
            rclcpp::shutdown();
            return 0;
        }
    } catch (const std::exception& e) {
        RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what());
        rclcpp::shutdown();
        return 1;
    }

    // Wait for calib.yaml file to be generated by host_sdk_sample
    std::string calib_file = package_path + "/config/calib.yaml";
    RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str());
    
    int wait_count = 0;
    while (rclcpp::ok() && !fileExists(calib_file)) {
        if (wait_count % 10 == 0) {
            RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file...");
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
        wait_count++;
        
        // Timeout after 5 seconds
        if (wait_count > 10) {
            RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file");
            rclcpp::shutdown();
            return 1;
        }
    }
    
    if (!rclcpp::ok()) {
        RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found.");
        return 0;
    }
    
    RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node...");

    auto node = std::make_shared<CloudReprojectionRosNode>();

    RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started");

    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

#else
// ==================== ROS1 Implementation ====================

CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
    : nh_(nh), pnh_(pnh)
{
    loadParameters();

    ROS_INFO_STREAM("\n  cloud_slam_topic: " << cloud_slam_topic_
                << "\n  odometry_topic: " << odometry_topic_
                << "\n  wiwc_topic: " << wiwc_topic_
                << "\n  camera_image_topic: " << camera_image_topic_
                << "\n  sync_prefix: " << sync_topic_prefix_
                << "\n  overlay_compressed_topic: " << sync_overlay_image_topic_
                << "\n  combined_compressed_topic: " << combined_compressed_topic_
                << "\n  publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
                << "\n  send_overlay: " << (send_overlay_ ? "on" : "off"));

    cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
    odom_sub_.subscribe(nh_, odometry_topic_, 1);
    wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
    image_compressed_sub_.subscribe(nh_, camera_image_topic_, 1);

    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_compressed_sub_);
    sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4));

    sync_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_topic_, 1);
    sync_cloud_slam_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_slam_topic_, 1);
    sync_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_odom_topic_, 1);
    sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
    sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);

    if (send_overlay_) {
        overlay_compressed_pub_ =
            nh_.advertise<sensor_msgs::CompressedImage>(sync_overlay_image_topic_, 1);
    }
    if (publish_combined_compressed_) {
        combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
    }

    ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
}

void CloudReprojectionRosNode::loadParameters()
{
    pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
    pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
    pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
    pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
    pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
    pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
    int jq = 85;
    pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
    combined_jpeg_quality_ = std::max(1, std::min(100, jq));
    int ojq = 85;
    pnh_.param<int>("register_keys/overlay_jpeg_quality", ojq, 85);
    overlay_jpeg_quality_ = std::max(1, std::min(100, ojq));
    int send_combined = 1;
    pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
    publish_combined_compressed_ = (send_combined != 0);
    int send_ov = 1;
    pnh_.param<int>("register_keys/send_overlay", send_ov, 1);
    send_overlay_ = (send_ov != 0);

    sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
    sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
    sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
    sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
    sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
    sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";

    // Load camera parameters
    CloudReprojector::CameraParams cam_params;
    pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
    pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
    pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
    pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
    pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
    pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
    pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
    pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
    pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
    pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
    pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
    pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
    pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);

    // Load extrinsic parameters
    CloudReprojector::ExtrinsicParams ext_params;
    std::vector<double> Tcl_vec_param;
    if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
    {
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j)
                ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
    }
    else
    {
        ROS_ERROR("Tcl_0 param missing or invalid.");
        ros::shutdown();
        return;
    }

    ext_params.Til = getFixedTil();
    ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);

    ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
    ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til);
    ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic);

    ROS_INFO("Camera intrinsics:");
    ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height);
    ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
             cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);

    reprojector_ = std::make_unique<CloudReprojector>();
    if (!reprojector_->initialize(cam_params, ext_params))
    {
        ROS_ERROR("Failed to initialize CloudReprojector");
        ros::shutdown();
    }
}

void CloudReprojectionRosNode::syncCallback(
    const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
    const nav_msgs::OdometryConstPtr& odom_msg,
    const nav_msgs::OdometryConstPtr& wiwc_msg,
    const sensor_msgs::CompressedImageConstPtr& image_compressed_msg)
{
    sync_cloud_slam_pub_.publish(cloud_msg);
    sync_odom_pub_.publish(odom_msg);
    sync_wiwc_pub_.publish(wiwc_msg);
    sync_image_pub_.publish(image_compressed_msg);

    pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
    pcl::fromROSMsg(*cloud_msg, cloud_odom);

    if (cloud_odom.empty())
    {
        ROS_WARN("Empty cloud_slam received");
        return;
    }

    Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
    for (int i = 0; i < 16; ++i) {
        T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
        T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
    }

    bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
    bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
    if (T_CL_valid && T_IL_valid) {
        reprojector_->updateExtrinsics(T_CL, T_IL);
    }

    CloudReprojector::OdomPose odom_pose;
    odom_pose.orientation = Eigen::Quaterniond(
        odom_msg->pose.pose.orientation.w,
        odom_msg->pose.pose.orientation.x,
        odom_msg->pose.pose.orientation.y,
        odom_msg->pose.pose.orientation.z
    );
    odom_pose.position = Eigen::Vector3d(
        odom_msg->pose.pose.position.x,
        odom_msg->pose.pose.position.y,
        odom_msg->pose.pose.position.z
    );

    pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
        reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
    if (cloud_cam.empty())
    {
        ROS_WARN("Camera-frame cloud is empty after reprojection");
        return;
    }

    sensor_msgs::PointCloud2 cloud_cam_msg;
    pcl::toROSMsg(cloud_cam, cloud_cam_msg);
    cloud_cam_msg.header = image_compressed_msg->header;
    if (cloud_cam_msg.header.frame_id.empty()) {
        cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
    }
    sync_cloud_pub_.publish(cloud_cam_msg);

    const bool need_combined = publish_combined_compressed_;
    cv::Mat depth_vis;
    if (need_combined) {
        depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
        if (depth_vis.empty()) {
            return;
        }
    }

    cv::Mat cam_bgr;
    if (send_overlay_ || need_combined) {
        cam_bgr = decodeCompressedToBgr(
            image_compressed_msg->data.data(), image_compressed_msg->data.size());
        if (cam_bgr.empty()) {
            ROS_ERROR("cv::imdecode failed (sync compressed image, format=%s)",
                      image_compressed_msg->format.c_str());
            return;
        }
    }

    if (send_overlay_) {
        cv::Mat overlay_vis =
            overlayProjectedCloudOnImage(cam_bgr, cloud_cam, reprojector_->getCameraParams());
        if (!overlay_vis.empty()) {
            std::vector<uchar> obuf;
            const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
            if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
                ROS_ERROR("cv::imencode failed (overlay)");
                return;
            }
            sensor_msgs::CompressedImage omsg;
            omsg.header = image_compressed_msg->header;
            omsg.format = "jpeg";
            omsg.data.assign(obuf.begin(), obuf.end());
            overlay_compressed_pub_.publish(omsg);
        }
    }

    if (publish_combined_compressed_) {
        const int H = std::max(depth_vis.rows, cam_bgr.rows);
        cv::Mat left = resizeToHeight(depth_vis, H);
        cv::Mat right = resizeToHeight(cam_bgr, H);
        cv::Mat combined;
        cv::hconcat(left, right, combined);

        std::vector<uchar> buf;
        const std::vector<int> enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_};
        if (!cv::imencode(".jpg", combined, buf, enc_params)) {
            ROS_ERROR("cv::imencode failed");
            return;
        }

        sensor_msgs::CompressedImage out;
        out.header = image_compressed_msg->header;
        out.format = "jpeg";
        out.data.assign(buf.begin(), buf.end());
        combined_pub_.publish(out);
    }
}

// ==================== ROS1 Main ====================
int main(int argc, char **argv)
{
    ros::init(argc, argv, "cloud_reprojection");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");
    
    // Check if reprojection is enabled
    int sendreprojection = 0;
    pnh.param("register_keys/sendreprojection", sendreprojection, 0);
    if(sendreprojection == 0)
    {
        ROS_INFO("Cloud reprojection will not be published.");
        return 0;
    }
    
    std::string calib_file_path;
    pnh.param<std::string>("calib_file_path", calib_file_path, "");
    
    ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
    while(ros::ok() && !fileExists(calib_file_path))
    {
        ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
        ros::Duration(0.5).sleep(); 
        ros::spinOnce();
    }
    
    if(!ros::ok())
    {
        ROS_INFO("Node shutdown before calib.yaml file was found.");
        return 0;
    }
    
    ROS_INFO("Found calib.yaml file! Loading parameters...");
    
    std::string node_name = ros::this_node::getName();
    std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
    int result = system(rosparam_command.c_str());
    
    if(result == 0)
    {
        ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
    }
    else
    {
        ROS_ERROR("Failed to load parameters from calib.yaml");
        return 1;
    }
    
    CloudReprojectionRosNode reprojection_node(nh, pnh);
    ros::spin();
    return 0;
}
#endif