#include <cinttypes>
#include <memory>
#include <algorithm>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "moveit/move_group_interface/move_group_interface.h"

#include "rclcpp/rclcpp.hpp"

using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr;

void handle_service(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<AddPlanningSceneObject::Request> request,
    const std::shared_ptr<AddPlanningSceneObject::Response> response)
{
    (void)request_header;
    // Create collision object for the robot to avoid
    auto const collision_object = [frame_id =
                                    "world", object_name=request->object_id, object_pose=request->object_pose] {
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = frame_id;
    collision_object.id = object_name;
    shape_msgs::msg::SolidPrimitive primitive;

    // Define the size of the box in meters
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.05;
    primitive.dimensions[primitive.BOX_Y] = 0.05;
    primitive.dimensions[primitive.BOX_Z] = 0.05;

    // Define the pose of the box (relative to the frame_id)
    geometry_msgs::msg::Pose box_pose;
    box_pose.position.x = object_pose[0];
    box_pose.position.y = object_pose[1];
    box_pose.position.z = object_pose[2];
    box_pose.orientation.x = object_pose[3];
    box_pose.orientation.y = object_pose[4];
    box_pose.orientation.z = object_pose[5];
    box_pose.orientation.w = object_pose[6];
    


    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    return collision_object;
    }();

    // Add the collision object to the scene
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    planning_scene_interface.applyCollisionObject(collision_object);

    response->success = true;
}

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions node_options;
    node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
    g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
    auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service);
    rclcpp::spin(g_node);
    rclcpp::shutdown();
    g_node = nullptr;
    return 0;
}