#include <cstdio>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

//#include <opencv2/opencv.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/highgui/highgui.hpp>

using namespace std::placeholders;
 
class MinimalDepthSubscriber : public rclcpp::Node {
  public:
    MinimalDepthSubscriber()
        : Node("depthviewer_node") {

        rclcpp::QoS depth_qos(10);
        depth_qos.keep_last(10);
        depth_qos.best_effort();
        depth_qos.durability_volatile();

        // Create depth map subscriber
        mDepthSub = create_subscription<sensor_msgs::msg::Image>(
                   "depth_image", depth_qos,
                   std::bind(&MinimalDepthSubscriber::depthCallback, this, _1) );
    }

  protected:
    void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
        float* depths = (float*)(&msg->data[0]);

        // Image coordinates of the center pixel
        int u = msg->width / 2;
        int v = msg->height / 2;

        // Linear index of the center pixel
        int centerIdx = u + msg->width * v;

        // Output the measure
        RCLCPP_INFO(get_logger(), "Center distance : %g m", depths[centerIdx]);
    }
    
    /* void depthCallback2(const sensor_msgs::msg::Image::SharedPtr msg) {
        float* depths = (float*)(&msg->data[0]);

	cv_bridge::CvImagePtr cv_ptr;
	cv_ptr = cv_bridge::toCvShare(msg, msg->encoding ); // of sensor_msgs::image_encodings::TYPE_32FC1
	
	double minVal, maxVal;
	minMaxLoc(cv_ptr->image, &minVal, &maxVal);
	    //Mat draw;
	cv::Mat blur_img; // opencv Mat
    	cv_ptr->image.convertTo(blur_img, CV_8U, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));

	cv::Mat resultMap;
	applyColorMap(data8uc, resultMap, cv::COLORMAP_JET); // cv::COLORMAP_AUTUMN
	cv::imshow("Jet-colormap", resultMap);

    }*/
    

  private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr mDepthSub;
};

// The main function
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

    auto depth_node = std::make_shared<MinimalDepthSubscriber>();

    rclcpp::spin(depth_node);
    rclcpp::shutdown();
    return 0;
}

