#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include "interfaces/msg/img_msg.hpp"
#include "img_dev/img_cfg.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.hpp>
#include <message_filters/synchronizer.h>
#include"img_dev/img_dev_node.hpp"
#include "realsense2_camera_msgs/msg/rgbd.hpp"

#include <vector>
#include<unordered_map>
using namespace std;
using namespace img_dev;
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,sensor_msgs::msg::Image>;
static shared_ptr<ImageSubscriber> cur_node=nullptr;
ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera/color/image_raw","/camera/depth/image_raw");
ImgCfg cfg1=ImgCfg("orbbec", "myType","top","/camera_02/color/image_raw","/camera_02/depth/image_raw");
//
ImgCfg cfg2=ImgCfg("realsense", "myType","right","/camera/camera/rgbd","/camera/camera/rgbd");
ImgCfg cfg3=ImgCfg("realsense", "myType","hand_r","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
void sync_cb0(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
    cur_node->pub_msg(c_img,d_img,cfg0);
}
void sync_cb1(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
    cur_node->pub_msg(c_img,d_img,cfg1);
}
int main(int argc, char* argv[]) {
    setenv("ROS_DISABLE_SHM", "1", 1); //禁用共享内存
    rclcpp::init(argc, argv);
    cur_node=make_shared<ImageSubscriber>();
    if(!cur_node){
        RCLCPP_ERROR(rclcpp::get_logger("main"),"img_dev node create error!!!!");
        return -1;
    }

    #if 0
    //////////////////奥比中光1 START///////////////////////
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera01_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera/color/camera_info", 10,
        [&](const sensor_msgs::msg::CameraInfo& info){
            if(info.k.empty()||info.d.empty())
                return;
            if(cfg0.k_arr.size()==0||cfg0.d_arr.size()==0){
                cfg0.k_arr.assign(info.k.begin(), info.k.end());
                cfg0.d_arr.assign(info.d.begin(), info.d.end());
                cfg0.arr_copy+=1;
                cout<<"copy01:"<<cfg0.arr_copy<<"k:"<<info.k[0]<<"d:"<<info.d[0]<<endl;
            }else{
                cfg0.arr_copy=10;
                ////cout<<"copy finish:"<<cfg0.arr_copy<<"k:"<<cfg0.k_arr.size()<<"d:"<<cfg0.d_arr.size()<<endl;
            }   
    });
    message_filters::Subscriber<sensor_msgs::msg::Image> c_sub0(cur_node, cfg0.getColorTopic());
    message_filters::Subscriber<sensor_msgs::msg::Image> d_sub0(cur_node,cfg0.getDepthTopic());
    auto sync0 = make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), c_sub0, d_sub0);
    sync0->registerCallback(&sync_cb0);
    //////////////////奥比中光1 END///////////////////////
#endif
   #if 0

    //////////////////奥比中光2 START///////////////////////
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera02_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera_02/color/camera_info", 10,
        [&](const sensor_msgs::msg::CameraInfo& info){
            if(info.k.empty()||info.d.empty())
                return;
            if(cfg1.k_arr.size()==0||cfg1.d_arr.size()==0){
                cfg1.k_arr.assign(info.k.begin(), info.k.end());
                cfg1.d_arr.assign(info.d.begin(), info.d.end());
                cfg1.arr_copy+=1;
                cout<<"copy02:"<<cfg1.arr_copy<<"k:"<<info.k[0]<<"d:"<<info.d[0]<<endl;
            }else{
                cfg1.arr_copy=10;
            }   
    });
    message_filters::Subscriber<sensor_msgs::msg::Image> c_sub1(cur_node, cfg1.getColorTopic());
    message_filters::Subscriber<sensor_msgs::msg::Image> d_sub1(cur_node,cfg1.getDepthTopic());
    auto sync1 = make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), c_sub1, d_sub1);
    sync1->registerCallback(&sync_cb1);
    //////////////////奥比中光2 END///////////////////////
#endif
    //////////////////realsense START///////////////////////
    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd1_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera/camera/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
        if(cfg2.k_arr.size()==0||cfg2.d_arr.size()==0){
                cfg2.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
                cfg2.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
                cfg2.arr_copy+=1;
                cout<<"copy:"<<"k:"<<cfg2.k_arr[0]<<"d:"<<cfg2.d_arr[0]<<endl;
        }
        cur_node->pub_msg(msg.rgb,msg.depth,cfg2);
      });
    //////////////////realsense END///////////////////////
#if 0
//////////////////realsense START///////////////////////
    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd2_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera2/camera2/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
        if(cfg3.k_arr.size()==0||cfg3.d_arr.size()==0){
                cfg3.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
                cfg3.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
                cfg3.arr_copy+=1;
                cout<<"copy:"<<"k:"<<cfg3.k_arr[0]<<"d:"<<cfg3.d_arr[0]<<endl;
        }
        cur_node->pub_msg(msg.rgb,msg.depth,cfg3);
      });
    //////////////////realsense END///////////////////////
#endif




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