#include <string>
#include <nlohmann/json.hpp>
#include <Eigen/Dense>

#include "rclcpp/rclcpp.hpp"
#include "message_filters/subscriber.hpp"
#include "message_filters/sync_policies/approximate_time.hpp"

#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/camera_info.hpp"

#include "interfaces/srv/vision_object_recognition.hpp"
#include "interfaces/msg/pose_array_class_and_id.hpp"


class VisionDetectNode: public rclcpp::Node {
    public: explicit VisionDetectNode(const std::string& name);
    
    private:
        bool output_box, output_mask;
        std::string get_camera_mode, detect_mode, calculate_mode;
        std::string cfg_str;
        nlohmann::json calibrition_path; 
        nlohmann::json get_camera_configs, segmentation_configs, calculate_configs;

        Eigen::Matrix4d left_hand_mat, right_hand_mat, head_mat;

        void (VisionDetectNode::*detect_function)();
        void (VisionDetectNode::*calculate_function)();

        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_detect_img;
        rclcpp::Publisher<interfaces::msg::PoseArrayClassAndID>::SharedPtr pub_pose_list;

        message_filters::Subscriber<sensor_msgs::msg::Image> color_img_sub;
        message_filters::Subscriber<sensor_msgs::msg::Image> depth_img_sub;
        std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image>>> sync_sub;

        rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub;

        rclcpp::Service<interfaces::srv::VisionObjectRecognition>::SharedPtr detect_server;
        
        void init_param();
        void init_model();
        void init_calibration_mat();
        void init_service();
        void init_topic();
        void init_publisher();
        void init_subscriber();

        void service_callback(
            const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request, 
            std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
        );

        void topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> &msg);

        void camera_info_callback(const std::shared_ptr<sensor_msgs::msg::CameraInfo> &msg);

        void seg_img();
        void seg_color();
        void seg_crossboard();
};