#include "vision_test/VisionDetectNode.hpp"
#include "vision_test/utils.hpp"

using json = nlohmann::json;
using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image>;

VisionDetectNode::VisionDetectNode(const std::string& name): Node(name) {
    /**/
    this->init_param();
    this->init_calibration_mat();

    if (this->output_box || this->output_mask) {
        this->init_publisher();
    }

    if (this->get_camera_mode == "Service") {
        this->get_camera_configs = json::parse(this->get_parameter("ServiceConfigs").as_string());
        this->init_service();
        this->init_subscriber();
    }
    else if (this->get_camera_mode == "Topic") {
        this->get_camera_configs = json::parse(this->get_parameter("TopicConfigs").as_string());
        this->init_topic();
    }
    else {
        RCLCPP_WARN(this->get_logger(), "Unkonw get_camera_mode, Use Service");
        this->get_camera_configs = json::parse(this->get_parameter("ServiceConfigs").as_string());
        this->init_service();
        this->init_subscriber();
    }

    if (this->detect_mode == "Detect") {
        this->init_model();
        this->segmentation_configs = json::parse(this->get_parameter("DetectConfigs").as_string());
        this->detect_function = &VisionDetectNode::seg_img;
    }
    else if (this->detect_mode == "Color") {
        this->segmentation_configs = json::parse(this->get_parameter("ColorConfigs").as_string());
        this->detect_function = &VisionDetectNode::seg_color;
    }
    else if (this->detect_mode == "Crossboard") {
        this->segmentation_configs = json::parse(this->get_parameter("CrossboardConfigs").as_string());
        this->detect_function = &VisionDetectNode::seg_crossboard;
    }
    else {
        RCLCPP_WARN(this->get_logger(), "Unkonw detect_mode, Use Service");                
        this->segmentation_configs = json::parse(this->get_parameter("DetectConfigs").as_string());
    }
    RCLCPP_INFO(this->get_logger(), "Init process done");
}


void VisionDetectNode::init_param() {
    /*Init Param*/
    this->declare_parameter<bool>("output_box", true);
    this->declare_parameter<bool>("output_mask", false);

    this->output_box = this->get_parameter("output_box").as_bool();
    this->output_mask = this->get_parameter("output_mask").as_bool();

    this->declare_parameter<std::string>("get_camera_mode", "Service");
    this->declare_parameter<std::string>("detect_mode", "Detect");
    this->declare_parameter<std::string>("calculate_mode", "PCA");

    this->get_camera_mode = this->get_parameter("get_camera_mode").as_string();
    this->detect_mode = this->get_parameter("detect_mode").as_string();
    this->calculate_mode = this->get_parameter("calculate_mode").as_string();

    this->declare_parameter<std::string>("calibrition_path", "{}");

    this->calibrition_path = json::parse(this->get_parameter("calibrition_path").as_string());

    this->declare_parameter<std::string>("ServiceConfigs", "{}");
    this->declare_parameter<std::string>("TopicConfigs", "{}");

    this->declare_parameter<std::string>("DetectConfigs", "{}");
    this->declare_parameter<std::string>("ColorConfigs", "{}");
    this->declare_parameter<std::string>("CrossboardConfigs", "{}");

    this->declare_parameter<std::string>("PCAConfigs", "{}");
    this->declare_parameter<std::string>("ICPConfigs", "{}");
    

    RCLCPP_INFO(this->get_logger(), "Init param done");
    // Done
}


void VisionDetectNode::init_model() {
    /*Init Model*/
    RCLCPP_INFO(this->get_logger(), "Init model done");
}


void VisionDetectNode::init_calibration_mat() {
    /*Init Calibration Mat*/
    std::string info;

    info = "left";
    this->left_hand_mat = read_data_from_json(this->calibrition_path["left_hand"].get<std::string>(), info);
    RCLCPP_INFO(this->get_logger(), info.c_str());

    info = "right";
    this->right_hand_mat = read_data_from_json(this->calibrition_path["right_hand"].get<std::string>(), info);
    RCLCPP_INFO(this->get_logger(), info.c_str());

    info = "head";
    this->head_mat = read_data_from_json(this->calibrition_path["head"].get<std::string>(), info);
    RCLCPP_INFO(this->get_logger(), info.c_str());

    RCLCPP_INFO(this->get_logger(), "Init calibration mat done");
    // Done
}


void VisionDetectNode::init_service() {
    /*Init_service*/
    detect_server = this->create_service<interfaces::srv::VisionObjectRecognition>(
        this->get_camera_configs["service_name"].get<std::string>(), 
        std::bind(
            &VisionDetectNode::service_callback, 
            this, 
            std::placeholders::_1, 
            std::placeholders::_2
        )
    );
   
    RCLCPP_INFO(this->get_logger(), "Init service done");
    // Done
}


void VisionDetectNode::init_topic() {
    /*Init Topic*/
    this->color_img_sub.subscribe(this, this->get_camera_configs["color_image_topic_name"].get<std::string>());
    this->depth_img_sub.subscribe(this, this->get_camera_configs["depth_image_topic_name"].get<std::string>());
    this->sync_sub = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
        SyncPolicy(10), 
        color_img_sub, depth_img_sub
    );

    this->sync_sub->registerCallback(&VisionDetectNode::topic_callback, this);

    this->camera_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>(
        this->get_camera_configs["camera_info_topic_name"].get<std::string>(), 
        10,
        std::bind(&VisionDetectNode::camera_info_callback, this, std::placeholders::_1)
    );

    RCLCPP_INFO(this->get_logger(), "Init topic done");
    // Done
}


void VisionDetectNode::init_publisher() {
    /*Init Publisher*/
    pub_detect_img = this->create_publisher<sensor_msgs::msg::Image>("/image/detect_image", 10);

    if (get_camera_mode == "Topic") {
        pub_pose_list = this->create_publisher<interfaces::msg::PoseArrayClassAndID>("/pose/cv_detect_pose", 10);
    }

    RCLCPP_INFO(this->get_logger(), "Init publisher done");
}


void VisionDetectNode::init_subscriber() {
    /*Init Subscriber*/
    RCLCPP_INFO(this->get_logger(), "Init subscriber done");
}


void VisionDetectNode::service_callback(
    const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request, 
    std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
) {
    /**/

}


void VisionDetectNode::topic_callback(const std::shared_ptr<sensor_msgs::msg::Image> &msg) {
    /**/

}


void VisionDetectNode::camera_info_callback(const std::shared_ptr<sensor_msgs::msg::CameraInfo> &msg) {
    /**/
    
}


void VisionDetectNode::seg_img() {
    /**/

}


void VisionDetectNode::seg_color() {
    /**/

}


void VisionDetectNode::seg_crossboard() {
    /**/

}





