/*
 * Copyright (C) 2023, Itamar Mishani
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Carnegie Mellon University nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/*!
 * \file   moveit_scene_interface.hpp
 * \author Itamar Mishani (imishani@cmu.edu)
 * \date   4/3/23
 */

#ifndef MANIPULATION_PLANNING_MOVEITINTERFACE_HPP
#define MANIPULATION_PLANNING_MOVEITINTERFACE_HPP

// include standard libraries
#include <iostream>
#include <vector>
// For boost:str and boost::fmt
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include<unordered_set>

// include ROS libraries
#include <eigen_conversions/eigen_msg.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <ros/ros.h>

#include <srmp_moveit_plugin/common/moveit_distance_field_wrapper.hpp>
#include <srmp_moveit_plugin/common/moveit_planning_scene_wrapper.hpp>
#include <manipulation_planning/scene_interface/moveit_scene_interface_base.hpp>
#include <search/common/world_objects.hpp>

namespace ims {

/// @class MoveitInterface
/// @brief A class that implements the SceneInterface for Moveit
/// @details This class implements the SceneInterface for Moveit. It is used to get the current state of the robot and to
/// get all information needed about the scene (e.g. obstacles, robot, etc.)
class MoveitInterface : public MoveitInterfaceBase {
private:
    // ====================
    // Private Variables.
    // ====================
    bool verbose_ = false;
    // The number of collision checks carried out.
    int num_collision_checks_ = 0;

public:
    // ====================
    // Public Variables.
    // ====================
    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
    std::shared_ptr<planning_scene::PlanningScene> planning_scene_;
    std::shared_ptr<moveit::planning_interface::PlanningSceneInterface> planning_scene_interface_;
    std::string group_name_;
    std::string group_name_ee_;
    // The names of any child move groups -- those are rooted on a non-base link of the robot.
    std::unordered_set<std::string> all_child_group_names_in_scene_;
    moveit::core::RobotStatePtr kinematic_state_;
    const moveit::core::JointModelGroup *joint_model_group_;
    std::vector<std::string> joint_names_;
    std::vector<std::pair<double, double>> joint_limits_;
    size_t num_joints_;
    std::string frame_id_;

    /// @brief Ordered names of the move groups in the scene. Agent0 is at the zero index, agent1 at the first index, etc.
    std::vector<std::string> scene_move_group_names_;

    // ====================
    // Public Methods.
    // ====================
    /// @brief Constructor
    explicit MoveitInterface(const std::string &group_name) : MoveitInterfaceBase(group_name) {
        // planning scene monitor
        planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
        planning_scene_monitor_->startSceneMonitor();
        planning_scene_monitor_->startStateMonitor();
        planning_scene_monitor_->startWorldGeometryMonitor();
        planning_scene_monitor_->requestPlanningSceneState();
        ros::Duration(0.3).sleep();
        planning_scene_ = planning_scene_monitor_->getPlanningScene();
        group_name_ = group_name;

        kinematic_state_ = std::make_shared<moveit::core::RobotState>(planning_scene_->getCurrentState());
        // Joint model group.
        joint_model_group_ = kinematic_state_->getJointModelGroup(group_name_);
        // Joint names.
        joint_names_ = joint_model_group_->getVariableNames();
        // The number of joints.
        num_joints_ = joint_names_.size();
        // Get the names of the objects in the scene.
        auto object_names = planning_scene_->getWorld()->getObjectIds();
        // Store the joint limits.
        getJointLimits(joint_limits_);
        // Get the names of the child move groups.
        getAllChildMoveGroupNames(all_child_group_names_in_scene_);

        // Store the frame id.
        frame_id_ = planning_scene_->getPlanningFrame();

        // Instantiate a planning scene interface.
        planning_scene_interface_ = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();

        // Get the names for all the move groups in the scene.
        scene_move_group_names_ = planning_scene_->getRobotModel()->getJointModelGroupNames();

        for (auto &obj : object_names) {
            std::cout << "Object name: " << obj << std::endl;
        }
        if (object_names.empty()) {
            std::cout << "No collision objects in the scene" << std::endl;
        }
    };

    /// @brief Constructor with the option to set the planning scene.
    MoveitInterface(const std::string &group_name, planning_scene::PlanningScenePtr &planning_scene) : MoveitInterfaceBase(group_name) {
        // planning scene monitor
        planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
        planning_scene_monitor_->startSceneMonitor();
        planning_scene_monitor_->startStateMonitor();
        planning_scene_monitor_->startWorldGeometryMonitor();
//        planning_scene_monitor_->requestPlanningSceneState(); This is no good.
        planning_scene_ = planning_scene;
        group_name_ = group_name;
        frame_id_ = planning_scene_->getPlanningFrame();
        planning_scene_interface_ = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();

        kinematic_state_ = std::make_shared<moveit::core::RobotState>(planning_scene_->getCurrentState());
        // joint model group
        joint_model_group_ = kinematic_state_->getJointModelGroup(group_name_);
        // get the joint names
        joint_names_ = joint_model_group_->getVariableNames();
        // get the number of joints
        num_joints_ = joint_names_.size();
        // Get the names of the child move groups.
        getAllChildMoveGroupNames(all_child_group_names_in_scene_);

        // Store the joint limits.
        getJointLimits(joint_limits_);

        auto object_names = planning_scene_->getWorld()->getObjectIds();

        for (auto &obj : object_names) {
            std::cout << "Object name: " << obj << std::endl;
        }
        if (object_names.empty()) {
            std::cout << "No collision objects in the scene" << std::endl;
        }
    };

    /// @brief Destructor
    ~MoveitInterface() override = default;

    // MULTI MULTI MULTI.
    /// @brief check if the state is valid
    /// @param state The state to check
    /// @return True if the state is valid, false otherwise
    bool isStateValid(const StateType &state) override {
        // Set the state of the ego robot and reset the states of all others.
        robot_state::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        // Reset the scene.
//        setAllNonChildGroupsToDefaultValues();
        // Set the state of the ego robot.
        current_scene_state.setJointGroupPositions(group_name_, state);

        // Check for the joint limits and if the scene is valid.
        num_collision_checks_++;

        // Instead, do this with a collision request.
        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        collision_request.contacts = true;
        collision_request.max_contacts = 1000;
        collision_request.max_contacts_per_pair = 1;
        collision_request.group_name = group_name_;
        collision_request.verbose = verbose_;
        planning_scene_->checkCollision(collision_request, collision_result);
        return !collision_result.collision;
    }
    // END MULTI MULTI MULTI.

    // SINGLE SINGLE SINGLE.
//    /// @brief check if the state is valid
//    /// @param state The state to check
//    /// @return True if the state is valid, false otherwise
//    bool isStateValid(const StateType &state) {
//        moveit_msgs::RobotState robotState;
//        // copy the values
//        robotState.joint_state.position = state;
//        robotState.joint_state.name = joint_names_;
//
//        // Check for the joint limits and if the scene is valid.
//        num_collision_checks_++;
//        return !planning_scene_->isStateColliding(robotState, group_name_);
//    }
    // END SINGLE SINGLE SINGLE.

    /// @brief check if the state is valid w.r.t. all other bodies. Robot and non-robot.
    /// @param state the configuration to check
    /// @param collisions_collective the collisions that have been found, potentially.
    /// @return true if the state is valid, false otherwise.
    /// @note This function checks for the current state of the robot against the current state of the scene. The scene is not reset and not other robots' states are changed. If you want to test againts other robots, use the other isStateValid function.
    bool isStateValid(const StateType &state, CollisionsCollective &collisions_collective) override {
        // Check the passed state for joint limits.
        if (!isStateWithinJointLimits(state)) {
            return false;
        }

        // Set the state of the ego robot and reset the states of all others.
        robot_state::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        // Reset the scene.
//        setAllNonChildGroupsToDefaultValues();

        // Set the state of the ego robot.
        current_scene_state.setJointGroupPositions(group_name_, state);

        // Check if this configuration is in collision. Whether any robot collides with any other.
        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        collision_request.contacts = true;
        collision_request.max_contacts = 1000;
        collision_request.max_contacts_per_pair = 1;
        collision_request.group_name = group_name_;
        collision_request.verbose = verbose_;

//        // Itamar
//        moveit::core::RobotState robot_state(planning_scene_->getCurrentState());
//        moveit_msgs::RobotState robotState;
//        // copy the values
//        robotState.joint_state.position = state;
//        robotState.joint_state.name = joint_names_;
//        moveit::core::robotStateMsgToRobotState(planning_scene_->getTransforms(), robotState, robot_state);
//        //////

        planning_scene_->checkCollision(collision_request, collision_result, current_scene_state);
        num_collision_checks_++;

        // Convert the collision result to a collision collective.
        std::string agent_name_prefix = group_name_.substr(0, group_name_.find("_"));
        moveitCollisionResultToCollisionsCollective(collision_result, collisions_collective, agent_name_prefix);

        return collisions_collective.size() == 0;
    }

    /// @brief check if the state is valid w.r.t. other bodies.
    /// @param state the configuration to check
    /// @param other_move_group_names the names of the robots to check against.
    /// @param other_move_group_states the states of the robots to check against.
    /// @param collisions_collective the collisions that have been found, potentially.
    /// @return true if the state is valid, false otherwise.
    bool isStateValid(const StateType &state,
                      const std::vector<std::string> &other_move_group_names,
                      const std::vector<StateType> &other_move_group_states,
                      CollisionsCollective &collisions_collective) override {
        // Check the passed state for joint limits.
        if (!isStateWithinJointLimits(state)) {
            return false;
        }
//
//
//        // Itamar
//        moveit::core::RobotState robot_state(planning_scene_->getCurrentState());
//        moveit_msgs::RobotState robotState;
//        // copy the values
//        robotState.joint_state.position = state;
//        robotState.joint_state.name = joint_names_;
//        moveit::core::robotStateMsgToRobotState(planning_scene_->getTransforms(), robotState, robot_state);
//        //////

        // Set the state of all robots.
        robot_state::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
//        setAllNonChildGroupsToDefaultValues();

        int num_move_groups = other_move_group_names.size();
        for (int i = 0; i < num_move_groups; i++) {
            current_scene_state.setJointGroupPositions(other_move_group_names[i], other_move_group_states[i]);

        }

        // Set the state of the ego robot.
        current_scene_state.setJointGroupPositions(group_name_, state);

        // Check if this configuration is in collision. Whether any robot collides with any other.
        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        collision_request.contacts = true;
        collision_request.max_contacts = 1000;
        collision_request.max_contacts_per_pair = 1;
        collision_request.verbose = verbose_;
        collision_request.group_name = group_name_;

        planning_scene_->checkCollision(collision_request, collision_result, current_scene_state);
        num_collision_checks_++;

        // Convert the collision result to a collision collective.
        std::string agent_name_prefix = group_name_.substr(0, group_name_.find("_"));
        std::vector<std::string> other_agent_name_prefixes;
        for (auto &other_move_group_name : other_move_group_names) {
            std::string other_agent_name_prefix = other_move_group_name.substr(0, other_move_group_name.find("_"));
            other_agent_name_prefixes.push_back(other_agent_name_prefix);
        }
        moveitCollisionResultToCollisionsCollective(collision_result, collisions_collective, agent_name_prefix, other_agent_name_prefixes);

        return collisions_collective.size() == 0;
    }

    /// @brief check if the state is valid w.r.t. other bodies and other robots in given configurations.
    /// @param state
    /// @param other_move_group_names
    /// @param other_move_group_states
    /// @param sphere_world_objects
    /// @param collisions_collective
    /// @return
    bool isStateValid(const StateType &state,
                      const std::vector<std::string> &other_move_group_names,
                      const std::vector<StateType> &other_move_group_states,
                      const std::vector<SphereWorldObject> &sphere_world_objects,
                      CollisionsCollective &collisions_collective) override {
        // Add the objects to the scene. Keep track of the objects.
        std::vector<void*> object_msgs;
        addSpheresToScene(sphere_world_objects, object_msgs);

        // Call the other isStateValid function.
        bool is_valid = isStateValid(state, other_move_group_names, other_move_group_states, collisions_collective);

        // Remove the objects from the scene.
        removeObjectsFromScene(object_msgs);

        return is_valid;
    }

    /// @brief check if the state is valid w.r.t. spheres in it.
    /// @param state
    /// @param other_move_group_names
    /// @param other_move_group_states
    /// @param sphere_world_objects
    /// @param collisions_collective
    /// @return
    bool isStateValid(const StateType &state,
                      const std::vector<SphereWorldObject> &sphere_world_objects,
                      CollisionsCollective &collisions_collective) override {
        // Add the objects to the scene. Keep track of the objects.
        std::vector<void*> object_msgs;
        addSpheresToScene(sphere_world_objects, object_msgs);

        // Call the other isStateValid function.
        bool is_valid = isStateValid(state, collisions_collective);

        // Remove the objects from the scene.
        removeObjectsFromScene(object_msgs);

        return is_valid;
    }

    /// @brief A utility function for adding sphere collision object to a planning scene.
    /// @param sphere_world_objects
    /// @param object_msgs the collision objects that were added to the scene.
    /// @return nothing.
    void addSpheresToScene(const std::vector<SphereWorldObject> &sphere_world_objects, std::vector<void*> &object_msgs) override {

        for (auto &obj : sphere_world_objects) {
            // Add the object to the scene with a new name.
            moveit_msgs::CollisionObject collision_object;
            collision_object.id = "world_sphere" + std::to_string(object_msgs.size());
            collision_object.header.frame_id = frame_id_;
            shapes::ShapeMsg collision_object_shape_msg;
            auto *shape = new shapes::Sphere(obj.radius);
            shapes::constructMsgFromShape(shape, collision_object_shape_msg);
            geometry_msgs::Pose collision_object_pose;
            collision_object_pose.position.x = obj.origin.x();
            collision_object_pose.position.y = obj.origin.y();
            collision_object_pose.position.z = obj.origin.z();

            collision_object_pose.orientation.x = 0.0;
            collision_object_pose.orientation.y = 0.0;
            collision_object_pose.orientation.z = 0.0;
            collision_object_pose.orientation.w = 1.0;

            collision_object.primitives.push_back(boost::get<shape_msgs::SolidPrimitive>(collision_object_shape_msg));
            collision_object.primitive_poses.push_back(collision_object_pose);
            collision_object.operation = moveit_msgs::CollisionObject::ADD;

            // Update the planning scene.
            planning_scene_interface_->applyCollisionObject(collision_object);  // For visualization. TODO(yoraish): remove.
            planning_scene_->processCollisionObjectMsg(collision_object);       // For collision checking.

            // Add the object to the list of objects.
            object_msgs.push_back(static_cast<void*>(new moveit_msgs::CollisionObject(collision_object)));
        }
    }

    /// @brief Remove collision objects from the planning scene.
    /// @param object_msgs
    void removeObjectsFromScene(std::vector<void*> object_msgs) override {

        for (auto &obj_msg : object_msgs) {
            moveit_msgs::CollisionObject* collision_obj = static_cast<moveit_msgs::CollisionObject*>(obj_msg);
            collision_obj->operation = collision_obj->REMOVE;
            planning_scene_interface_->applyCollisionObject(*collision_obj);  // TODO(yoraish): For viz. Remove this.
            planning_scene_->processCollisionObjectMsg(*collision_obj);       // For collision checking.
        }
    }

    /// @brief check if the given state of the specified groups is valid.
    /// @param state the configuration to check
    /// @param other_move_group_names the names of the robots to check against.
    /// @param other_move_group_states the states of the robots to check against.
    /// @param collisions_collective the collisions that have been found, potentially.
    /// @return true if the state is in collision, false otherwise.
    bool checkCollision(const std::vector<std::string> &other_move_group_names,
                        const std::vector<StateType> &other_move_group_states,
                        CollisionsCollective &collisions_collective) override {
        // Set the state of all robots.
        robot_state::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        int num_move_groups = static_cast<int>(other_move_group_names.size());
        for (int i = 0; i < num_move_groups; i++) {
            current_scene_state.setJointGroupPositions(other_move_group_names[i], other_move_group_states[i]);
        }

        // Check if this configuration is in collision. Whether any robot collides with any other.
        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        collision_request.contacts = true;
        collision_request.max_contacts = 1000;
        collision_request.max_contacts_per_pair = 1;
        collision_request.verbose = verbose_;

        planning_scene_->checkCollision(collision_request, collision_result);
        num_collision_checks_++;

        // Convert the collision result to a collision collective.
        std::vector<std::string> other_agent_name_prefixes;
        for (auto &other_move_group_name : other_move_group_names) {
            std::string other_agent_name_prefix = other_move_group_name.substr(0, other_move_group_name.find("_"));
            other_agent_name_prefixes.push_back(other_agent_name_prefix);
        }
        moveitCollisionResultToCollisionsCollective(collision_result, collisions_collective, "", other_agent_name_prefixes);

        return collisions_collective.size() > 0;
    }

    /// @brief Check if a path is valid
    /// @param path The path to check
    /// @return True if the path is valid, false otherwise
    bool isPathValid(const PathType &path) override {
        // TODO: Is this ok or should i use isPathValid instead? This is great. And I think this should be moved to the action space such that the isStateValid function could be customized.
        return std::all_of(path.begin(), path.end(), [this](const StateType &state_val) { return isStateValid(state_val); });
        // return true;
    }

    /// @brief Calculate IK for a given pose
    /// @param pose The pose to calculate IK for
    /// @param joint_state_seed The joint state to use as a seed.
    /// @param joint_state The joint state to store the IK solution
    /// @param timeout The timeout for the IK calculation
    /// @return True if IK was found, false otherwise
    bool calculateIK(const Eigen::Isometry3d &pose,
                     const StateType &joint_state_seed,
                     StateType &joint_state,
                     double timeout = 0.1) override {
        // Convert the input to a geometry_msgs::Pose.
        geometry_msgs::Pose pose_msg;
        tf::poseEigenToMsg(pose, pose_msg);

        // Call the other calculateIK function.
        return calculateIK(pose_msg, joint_state_seed, joint_state, 3.0, timeout);  // Used to be 1.0, timeout);
    }

    /// @brief Calculate IK for a given pose
    /// @param pose The pose to calculate IK for
    /// @param joint_state The joint state to store the IK solution
    /// @param timeout The timeout for the IK calculation
    /// @return True if IK was found, false otherwise
    bool calculateIK(const geometry_msgs::Pose &pose,
                     StateType &joint_state,
                     double timeout = 0.1) override {
        // resize the joint state
        joint_state.resize(num_joints_);
        // set joint model group as random, only the relevant kinematic group
        kinematic_state_->setToRandomPositions(joint_model_group_);
        // update
        planning_scene_monitor_->updateFrameTransforms();
        planning_scene_monitor_->updateSceneWithCurrentState();  // TODO: Is this needed?
        // set the pose
        if (kinematic_state_->setFromIK(joint_model_group_, pose, timeout)) {
            // get the joint values
            kinematic_state_->copyJointGroupPositions(joint_model_group_, joint_state);
            return true;
        }
        else {
            ROS_INFO("No IK solution found without using seed");
            return false;
        }
    }

    /// @brief Calculate IK for a given pose and a given seed
    /// @param pose The pose to calculate IK for
    /// @param seed The seed to use for the IK
    /// @param joint_state The joint state to store the IK solution
    /// @param consistency_limit The consistency limit to use for the IK
    /// @param timeout The timeout for the IK calculation
    /// @return True if IK was found, false otherwise
    bool calculateIK(const geometry_msgs::Pose &pose,
                     const StateType &seed,
                     StateType &joint_state,
                     double consistency_limit = 1.0,
                     double timeout = 0.1) {
        // resize the joint state
        joint_state.resize(num_joints_);
        // set the pose
        kinematic_state_->setJointGroupPositions(joint_model_group_, seed);
        // update
        planning_scene_monitor_->updateFrameTransforms();
        planning_scene_monitor_->updateSceneWithCurrentState();
        // add a consistency_limits of 0.1 to the seed
        std::vector<double> consistency_limits(num_joints_, consistency_limit);
        // get the tip link
        const std::string &tip_link = joint_model_group_->getLinkModelNames().back();
        Eigen::Isometry3d pose_eigen;
        tf::poseMsgToEigen(pose, pose_eigen);

        if (kinematic_state_->setFromIK(joint_model_group_, pose_eigen,
                                        tip_link, consistency_limits,
                                        timeout)) {
            kinematic_state_->copyJointGroupPositions(joint_model_group_, joint_state);
            return true;
        }
        else {
            ROS_INFO("No IK solution found using seed");
            return false;
        }
    }

    /// @brief Calculate FK for a given joint state
    /// @param joint_state The joint state to calculate FK for
    /// @param pose The pose to store the FK solution
    /// @return True if FK was found, false otherwise
    bool calculateFK(const StateType &joint_state,
                     StateType &pose) {
        // set the joint state
        kinematic_state_->setJointGroupPositions(joint_model_group_, joint_state);
        // update
        planning_scene_monitor_->updateFrameTransforms();
        planning_scene_monitor_->updateSceneWithCurrentState();
        // get the tip link
        const std::string &tip_link = joint_model_group_->getLinkModelNames().back();
        // get the pose
        const Eigen::Isometry3d &end_effector_state = kinematic_state_->getGlobalLinkTransform(tip_link);
        // push to pose as a vector of x, y, z, r, p, y
        pose.resize(6);
        pose[0] = end_effector_state.translation().x();
        pose[1] = end_effector_state.translation().y();
        pose[2] = end_effector_state.translation().z();
        ims::get_euler_zyx(end_effector_state.rotation(), pose[5], pose[4], pose[3]);
        ims::normalize_euler_zyx(pose[5], pose[4], pose[3]);
        return true;
    }

    /// @brief get the joint limits
    /// @param joint_limits The joint limits to store the limits
    void getJointLimits(std::vector<std::pair<double, double>> &joint_limits) const override {  // TODO: check if this is the right way to do it
        const auto &bounds = joint_model_group_->getActiveJointModelsBounds();
        std::vector<double> min_vals, max_vals;
        for (int i = 0; i < num_joints_; i++) {
            const auto *jb = bounds[i];
            for (auto &b : *jb) {
                max_vals.push_back(b.max_position_);
                min_vals.push_back(b.min_position_);
            }
        }
        // resize the joint limits
        joint_limits.resize(num_joints_);
        // assert if the number of joint is not equal to the size of the bounds
        assert(num_joints_ == min_vals.size());
        for (int i = 0; i < num_joints_; i++) {
            joint_limits[i] = std::make_pair(min_vals[i], max_vals[i]);
        }
    }

    bool isStateWithinJointLimits(const StateType &state) const override {
        for (int i = 0; i < num_joints_; i++) {
            if (state.at(i) < joint_limits_.at(i).first || state.at(i) > joint_limits_.at(i).second) {
                return false;
            }
        }
        return true;
    }

    inline int getNumCollisionChecks() const override {
        return num_collision_checks_;
    }

    inline std::vector<std::string> getJointNames() const override {
        return joint_names_;
    }

    void getCurrentState(StateType& state_val) const override {
        kinematic_state_->copyJointGroupPositions(joint_model_group_, state_val);
    }

    std::string getGroupName() const override {
        return group_name_;
    }

    int getNumJoints() const override {
        return joint_model_group_->getVariableCount();
    }

    /// @brief Get the distance of a point to the robot.
    /// @param state The state of the robot.
    /// @param point The point to get the distance to, specified in the world frame (the planning frame of the scene).
    /// @param max_distance The maximum distance to check for. If a distance larger than this is found, the function returns this value.
    /// @param distance The distance to the robot to be populated by the function.
    void getDistanceToRobot(const StateType& state, const Eigen::Vector3d& point, double max_distance, double& distance) override {
        // Set the state of the ego robot and reset the states of all others.
        moveit::core::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        // Reset the scene.
//        setAllNonChildGroupsToDefaultValues();
        // Set the state of the ego robot.
        current_scene_state.setJointGroupPositions(group_name_, state);

        // Create a distance field around the point with a maximum distance of the radius.
        std::string robot_base_link_name = kinematic_state_->getJointModelGroup(group_name_)->getLinkModelNames().at(0);
        Eigen::Isometry3d robot_base_link_to_world = kinematic_state_->getFrameTransform(robot_base_link_name);
        double df_extent_x = max_distance * 2;
        double df_extent_y = max_distance * 2;
        double df_extent_z = max_distance * 2;
        double df_x_corner = point.x() - df_extent_x / 2.0;
        double df_y_corner = point.y() - df_extent_y / 2.0;
        double df_z_corner = point.z() - df_extent_z / 2.0;

        std::shared_ptr<distance_field::PropagationDistanceField> df = ims::getEmptyDistanceField(df_extent_x, df_extent_y, df_extent_z, 0.05, df_x_corner, df_y_corner, df_z_corner, max_distance);
        // Get the robot occupancy.
        moveit::core::RobotState current_state = planning_scene_->getCurrentState();

        assert (group_name_ee_ != "");

        std::vector<std::string> move_group_names {group_name_, group_name_ee_};
        ims::addRobotToDistanceField(df, current_state, move_group_names);
        std::chrono::steady_clock::time_point end_df = std::chrono::steady_clock::now();

        // Evaluate the distance of the point in the df.
        distance = df->getDistance(point.x(), point.y(), point.z());

        // If verbose then also visualize the distance field.
//        if (verbose_) {
//            ims::visualizeOccupancy(df, marker_pub, frame_id_);
//        }
    }

    /// @brief Get the distance of a point to the robot.
    /// @param state The state of the robot.
    /// @param point The point to get the distance to, specified in the world frame (the planning frame of the scene).
    /// @param max_distance The maximum distance to check for. If a distance larger than this is found, the function returns this value.
    /// @param distance The distance to the robot to be populated by the function.
    bool isRobotCollidingWithSphere(const StateType& state, const Eigen::Vector3d& point, double radius) override {
        // Set the state of the ego robot and reset the states of all others.
        moveit::core::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        // Reset the scene.
//        setAllNonChildGroupsToDefaultValues();
        // Set the state of the ego robot.
        current_scene_state.setJointGroupPositions(group_name_, state);

        // Add a sphere to the scene.
        moveit_msgs::CollisionObject collision_object;
        collision_object.id = "constraint_sphere";
        collision_object.header.frame_id = frame_id_;
        shapes::ShapeMsg collision_object_shape_msg;
        auto *shape = new shapes::Sphere(radius);
        shapes::constructMsgFromShape(shape, collision_object_shape_msg);
        geometry_msgs::Pose collision_object_pose;
        collision_object_pose.position.x = point.x();
        collision_object_pose.position.y = point.y();
        collision_object_pose.position.z = point.z();

        collision_object_pose.orientation.x = 0.0;
        collision_object_pose.orientation.y = 0.0;
        collision_object_pose.orientation.z = 0.0;
        collision_object_pose.orientation.w = 1.0;

        collision_object.primitives.push_back(boost::get<shape_msgs::SolidPrimitive>(collision_object_shape_msg));
        collision_object.primitive_poses.push_back(collision_object_pose);
        collision_object.operation = moveit_msgs::CollisionObject::ADD;

        // Update the planning scene.
        // planning_scene_interface_->applyCollisionObject(collision_object);  // For visualization. TODO: remove.
        planning_scene_->processCollisionObjectMsg(collision_object);       // For collision checking.

        // Check if the sphere is in collision.
        collision_detection::CollisionRequest collision_request;
        collision_detection::CollisionResult collision_result;
        collision_request.contacts = true;
        collision_request.max_contacts = 1;
        collision_request.max_contacts_per_pair = 1;
        collision_request.verbose = verbose_;
        collision_request.group_name = group_name_;

        planning_scene_->checkCollision(collision_request, collision_result);
        num_collision_checks_++;

        // Remove the sphere from the scene.
        collision_object.operation = collision_object.REMOVE;
        // planning_scene_interface_->applyCollisionObject(collision_object);  // For visualization. TODO: remove.
        planning_scene_->processCollisionObjectMsg(collision_object);       // For collision checking.

        return collision_result.collision;

    }

    void setEndEffectorMoveGroupName(const std::string& group_name_ee) override {
        group_name_ee_ = group_name_ee;
    }
    std::string getEndEffectorLinkName() const override {
        return joint_model_group_->getLinkModelNames().back();
    }

    std::shared_ptr<PlanningSceneInterface> getPlanningSceneMoveit() override {
        return std::make_shared<MoveItPlanningSceneWrapper>(planning_scene_);
    }

    /// @brief Attach a box to the end effector of the robot.
    /// \param origin_in_ee_frame
    /// \param size This is x, y, z [m] in the ee frame. In the Kinva Gen3 robots, the (tool frame) frame is
    /// x along the fingers axis and z along the axis away from the arm continuing the last link.
    void attachPrimitiveBoxObjectToEndEffector(const Eigen::Vector3d& origin_in_ee_frame, const Eigen::Vector3d& size) override {
        // Create a collision object for the box.
        moveit_msgs::CollisionObject collision_object;
        collision_object.header.frame_id = getEndEffectorLinkName();
        collision_object.id = group_name_ + "_attached_object";
        shape_msgs::SolidPrimitive box;
        box.type = box.BOX;
        box.dimensions.resize(3);
        box.dimensions[0] = size.x();
        box.dimensions[1] = size.y();
        box.dimensions[2] = size.z();
        collision_object.primitives.push_back(box);
        collision_object.operation = moveit_msgs::CollisionObject::ADD;
        geometry_msgs::Pose box_pose;
        box_pose.position.x = origin_in_ee_frame.x();
        box_pose.position.y = origin_in_ee_frame.y();
        box_pose.position.z = origin_in_ee_frame.z();
        box_pose.orientation.x = 0.0;
        box_pose.orientation.y = 0.0;
        box_pose.orientation.z = 0.0;
        box_pose.orientation.w = 1.0;
        collision_object.primitive_poses.push_back(box_pose);

        // Attach the object to the robot.
        planning_scene_interface_->applyCollisionObject(collision_object);
        planning_scene_->processCollisionObjectMsg(collision_object);

        moveit_msgs::AttachedCollisionObject attached_object;
        attached_object.link_name = getEndEffectorLinkName();
//        attached_object.touch_links = { joint_model_group_->getLinkModelNames().back() };
        attached_object.object = collision_object;
        attached_object.object.operation = moveit_msgs::CollisionObject::ADD;
        planning_scene_interface_->applyAttachedCollisionObject(attached_object);
        planning_scene_->processAttachedCollisionObjectMsg(attached_object);
        ros::Duration(0.1).sleep();
    }

    void removeObjectsAttachedToEndEffector() override {
        moveit_msgs::AttachedCollisionObject attached_object;
        // By convention, attached objects are prefixed by the name of the group they are attached to. e.g., panda0_arm_attached_object
        attached_object.link_name = getEndEffectorLinkName();
        attached_object.object.id = group_name_ + "_attached_object";
        attached_object.object.operation = moveit_msgs::CollisionObject::REMOVE;
        planning_scene_interface_->applyAttachedCollisionObject(attached_object);
        planning_scene_->processAttachedCollisionObjectMsg(attached_object);
        // Remove from the scene as well.
        planning_scene_interface_->applyCollisionObject(attached_object.object);
        planning_scene_->processCollisionObjectMsg(attached_object.object);
     }

    std::shared_ptr<DistanceFieldInterface> getDistanceFieldMoveItInterface(double df_size_x = 3.0,
                                                                                              double df_size_y = 3.0,
                                                                                              double df_size_z = 3.0,
                                                                                              double df_res = 0.02,
                                                                                              double df_origin_x = -0.75,
                                                                                              double df_origin_y = -1.5,
                                                                                              double df_origin_z = 0.0,
                                                                                              double max_distance = 1.8) {
        std::chrono::steady_clock::time_point start_df = std::chrono::steady_clock::now();
        auto df = std::make_shared<distance_field::PropagationDistanceField>(df_size_x, df_size_y, df_size_z,
                                                                             df_res, df_origin_x, df_origin_y, df_origin_z,
                                                                             max_distance);
        ROS_DEBUG_STREAM_NAMED("IMS", "Distance field size: " << df->getSizeX() << ", " << df->getSizeY() << ", " << df->getSizeZ());
        auto objects = planning_scene_->getWorld()->getObjectIds();
        ROS_INFO_STREAM_NAMED("IMS", "Number of objects in the scene: " << objects.size());
        std::vector<moveit_msgs::CollisionObject> objs;
        planning_scene_->getCollisionObjectMsgs(objs);
        // fill the distance field
        for (auto& obj : objs) {

            Eigen::Isometry3d pose;
            // convert from pose to eigen
            geometry_msgs::Pose pose_msg = obj.pose;
            tf::poseMsgToEigen(pose_msg, pose);
            shapes::Shape* shape;
            // get shape from a collision object
            if (obj.primitives.empty() && obj.meshes.empty()) {
                // raise exception
                ROS_ERROR("No shape found in the collision object");
                std::exception e;
            }
            else if (!obj.primitives.empty()){
                // Construct shape from primitive;
                shape = shapes::constructShapeFromMsg(obj.primitives[0]);
            }
            else {
                shape = shapes::constructShapeFromMsg(obj.meshes[0]);
            }
            df->addShapeToField(shape, pose);
        }
        std::chrono::steady_clock::time_point end_df = std::chrono::steady_clock::now();
        std::cout << "Distance field creation time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end_df - start_df).count() << " ms" << std::endl;
        return std::make_shared<MoveItDistanceFieldWrapper>(df);
    }

    /***
     * @brief Gets all group names in the scene that are children of ANY other move group. For example, if there are
     * multiple arms in the scene, this method will return all of their end effectors.
     * @param parent_group_name
     * @param child_group_names
     */
    void getAllChildMoveGroupNames(std::unordered_set<std::string>& all_child_group_names) override {
        all_child_group_names.clear();
        for (std::string parent_group_name: planning_scene_->getRobotModel()->getJointModelGroupNames()) {
            std::vector <std::string> child_group_names;
            getChildMoveGroupNames(parent_group_name, child_group_names);
            all_child_group_names.insert(child_group_names.begin(), child_group_names.end());
        }
        std::cout << "All child groups: " << all_child_group_names << std::endl;
    }

    void getChildMoveGroupNames(std::string parent_group_name, std::vector<std::string>& child_group_names) override {
        child_group_names.clear();
        // Get the JointModelGroup for the given move group.
        auto robot_model = planning_scene_->getRobotModel();
        const moveit::core::JointModelGroup* parent_jmg = robot_model->getJointModelGroup(parent_group_name);
        if (!parent_jmg){
            throw std::runtime_error(boost::str(boost::format("[getChildMoveGroupNames] Move group '%s' not found.") % parent_group_name));
        }

        // Get the base link of the parent move group.
        if (parent_jmg->getLinkModelNames().empty()){
            throw std::runtime_error(boost::str(boost::format("[getChildMoveGroupNames] Base link for move group '%s' not found.") % group_name_));
        }
        const moveit::core::LinkModel* parent_base_link = robot_model->getLinkModel(parent_jmg->getLinkModelNames().front());
        if (!parent_base_link)
            {
                throw std::runtime_error(boost::str(boost::format("[getChildMoveGroupNames] Base link for move group '%s' not found.") % parent_group_name));
        }

        // Get the first link of the group. Child links that start from the base link are not actual children.
        if (parent_jmg->getLinkModelNames().size() < 2){
            return;
        }
        const moveit::core::LinkModel* parent_first_link = robot_model->getLinkModel(*((parent_jmg->getLinkModelNames()).begin() + 1));

        ROS_INFO("Child groups of '%s':", parent_group_name.c_str());
        // Iterate through all move groups and check if they are children of the given group's first link.
        for (const std::string& group_name : robot_model->getJointModelGroupNames())
        {
            const moveit::core::JointModelGroup* candidate_jmg = robot_model->getJointModelGroup(group_name);
            if (candidate_jmg == parent_jmg) { continue; } // Skip the parent itself.

            // Get the base link of the candidate move group.
            const moveit::core::LinkModel* candidate_base_link = candidate_jmg->getLinkModelNames().empty() ? nullptr : robot_model->getLinkModel(candidate_jmg->getLinkModelNames().front());
            if (!candidate_base_link) { continue; }

            // Check if the candidate's base link is a descendant of the parent's first link.
            const moveit::core::LinkModel* current_link = candidate_base_link;
            while (current_link)
            {
                if (current_link == parent_first_link)
                {
                    // If this group's link is the same as the parent's first link, then it is a child.
                    child_group_names.push_back(group_name);
                    ROS_INFO("  - %s starting from candidate base link %s", group_name.c_str(), candidate_base_link->getName().c_str());
                    break;
                }
                current_link = current_link->getParentLinkModel();
            }
        }
    }

    /***
     * @brief This method sets all articulations (arms) to their default values. This does not affect any child-groups
     * that are attached to any of the articulations. This is because the child groups will generally not have their values
     * set later and may remain undefined.
     */
    void setAllNonChildGroupsToDefaultValues() override {
        robot_state::RobotState &current_scene_state = planning_scene_->getCurrentStateNonConst();
        // Record all the states of the "child" groups. Those are groups that are rooted at other groups.
        std::map<std::string, StateType> child_group_states;
        for (std::string child_group_name : all_child_group_names_in_scene_) {
            StateType child_group_state;
            current_scene_state.copyJointGroupPositions(child_group_name, child_group_state);
            child_group_states[child_group_name] = child_group_state;
        }
        // Reset the scene.
        current_scene_state.setToDefaultValues();
        // Set the states of the child groups back.
        for (const auto& child_group_state : child_group_states) {
            current_scene_state.setJointGroupPositions(child_group_state.first, child_group_state.second);
        }

    }

    std::string getPlanningFrame() const override {
        return planning_scene_->getPlanningFrame();
    }

    std::string getTipLinkName() const override {
        return joint_model_group_->getLinkModelNames().back();
    }

    Eigen::Isometry3d getGlobalTipLinkTransform() const override {
        return kinematic_state_->getGlobalLinkTransform(getTipLinkName());
    }


}; // Class.
}  // namespace ims

#endif  // MANIPULATION_PLANNING_MOVEITINTERFACE_HPP
