/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik, Inc.
 *  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 PickNik 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.
 *********************************************************************/

/* Author: Itamar Mishani (imishani@andrew.cmu), Yorai Shaoul (yorai@cmu.edu).
 * Date: Sep 7, 2023
 */

// Standard includes.
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <ros/ros.h>
#include <limits>
#include <utility>
#include <vector>
#include <Eigen/Geometry>
#include <unordered_map>

// Project includes.
#include "srmp_moveit_plugin/srmp_interface.hpp"
#include "srmp_moveit_plugin/common/factories.hpp"

namespace srmp_interface
{
constexpr char LOGNAME[] = "srmp_interface";

SRMPInterface::SRMPInterface(const std::string& context_name,
                           std::string  group_name,
                           const moveit::core::RobotModelConstPtr& robot_model,
                           const ros::NodeHandle& nh) : nh_(nh),
                                                        name_("SRMPInterface"),
                                                        group_name_(std::move(group_name)),
                                                        robot_model_(robot_model){
    ROS_INFO_STREAM_NAMED(LOGNAME, "Initializing SRMP Interface: " << context_name);
    planning_world_ = std::make_shared<ims::PlanningWorld>();
    // Get the total num of joint in the group. If this is two or less, then do not plan for this one.
    dof_ = static_cast<int>(robot_model_->getJointModelGroup(group_name_)->getVariableCount());
    if (dof_ <= 2) {
        ROS_WARN_STREAM_NAMED(LOGNAME, CYAN << "Group " << group_name_ << " has " << dof_ << " joints. Not planning for this group." << RESET);
        return;
    }

//    auto full_path = boost::filesystem::path(__FILE__).parent_path().parent_path();
//    auto full_path = ros::package::getPath("manipulation_planning");
//    path_mprim_ = full_path + "/config/manip_" + std::to_string(dof_) + "dof_mprim.yaml";
//    std::cout << BOLDGREEN << "Path to mprim: " << path_mprim_ << RESET << std::endl;

//    action_type_ = std::make_shared<ims::ManipulationType>(path_mprim_);
//    // create heuristic factories
//    heuristic_factories_["bfs"] = ims::MakeBFSHeuristic;
//    heuristic_factories_["joint_euclidean"] = ims::MakeJointAnglesHeuristic;
//    heuristic_factories_["joint_euclidean_remove_time"] = ims::MakeEuclideanRemoveTimeHeuristic;
//
//    // create planner and planner params factories
//    planner_factories_["Astar"] = ims::MakeAStar;
//    planner_factories_["wAstar"] = ims::MakeWAStar;
//    planner_factories_["ARAstar"] = ims::MakeARAstar;
//    planner_factories_["MHAstar"] = ims::MakeMHAstar;
//    // Multi-agent factories.
//    planner_factories_["ECBS"] = ims::MakeECBS;
//    multi_agent_planner_names_.insert("ECBS");

    loadPlannerConfigurations();

}

void SRMPInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig) {
    // based on the planner name, create a planner params
    // print all the planner configurations
    for (const std::pair<const std::string, planning_interface::PlannerConfigurationSettings>& config : pconfig)
    {
        ROS_INFO_STREAM_NAMED(LOGNAME, "Parameters for configuration '" << config.first << "'");
        for (const std::pair<const std::string, std::string>& parameters : config.second.config) {
            ROS_INFO_STREAM_NAMED(LOGNAME, " - " << parameters.first << " = " << parameters.second);
        }
    }

//    planner_name_ = pconfig.begin()->second.config.at("type");
//    heuristic_name_ = pconfig.begin()->second.config.at("heuristic");
    // find the heuristic factory
//    auto heuristic_factory = heuristic_factories_.find(heuristic_name_);
//    if (heuristic_factory == heuristic_factories_.end())
//    {
//        ROS_ERROR_NAMED(LOGNAME, "Could not find heuristic factory for '%s'", heuristic_name_.c_str());
//        throw std::runtime_error("Could not find heuristic factory");
//    } else {
//        ROS_INFO_STREAM_NAMED(LOGNAME, "Found heuristic factory for '" << heuristic_name_ << "'");
//    }
//    heuristic_ = heuristic_factory->second(pconfig.begin()->second);

//    auto planner_factory = planner_factories_.find(planner_name_);
//    if (planner_factory == planner_factories_.end())
//    {
//        ROS_ERROR_NAMED(LOGNAME, "Could not find planner factory for '%s'", planner_name_.c_str());
//        throw std::runtime_error("Could not find planner factory");
//    } else {
//        ROS_INFO_STREAM_NAMED(LOGNAME, "Found planner factory for '" << planner_name_ << "'");
//    }
//    planner_ = planner_factory->second(heuristic_.get(), pconfig.begin()->second);

//    double resolution = std::stod(pconfig.begin()->second.config.at("resolution"));
//    std::vector<double> discretization(dof_, resolution);
//    ims::deg2rad(discretization);
//
//    action_type_->Discretization(discretization);
    configuration_settings_ = pconfig.begin()->second;
    pconfig_ = pconfig;
}

bool SRMPInterface::loadPlannerConfiguration(
    const std::string& group_name, const std::string& planner_id,
    const std::map<std::string, std::string>& group_params,
    planning_interface::PlannerConfigurationSettings& planner_config)
{
    XmlRpc::XmlRpcValue xml_config;
    if (!nh_.getParam("planner_configs/" + planner_id, xml_config))
    {
        ROS_ERROR_NAMED(LOGNAME, "Could not find the planner configuration '%s' on the param server", planner_id.c_str());
        return false;
    }

    if (xml_config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
        ROS_ERROR_NAMED(LOGNAME, "A planning configuration should be of type XmlRpc Struct type (for configuration '%s')",
                        planner_id.c_str());
        return false;
    }

    planner_config.name = group_name + "[" + planner_id + "]";
    planner_config.group = group_name;

    // default to specified parameters of the group (overridden by configuration specific parameters)
    planner_config.config = group_params;

    // read parameters specific for this configuration
    for (std::pair<const std::string, XmlRpc::XmlRpcValue>& element : xml_config)
    {
        if (element.second.getType() == XmlRpc::XmlRpcValue::TypeString)
            planner_config.config[element.first] = static_cast<std::string>(element.second);
        else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
            planner_config.config[element.first] = moveit::core::toString(static_cast<double>(element.second));
        else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeInt)
            planner_config.config[element.first] = std::to_string(static_cast<int>(element.second));
        else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
            planner_config.config[element.first] = std::to_string(static_cast<bool>(element.second));
    }
    return true;
}

void SRMPInterface::loadPlannerConfigurations() {
    // read the planning configuration for each group
    planning_interface::PlannerConfigurationMap pconfig;
    pconfig.clear();
    // the set of planning parameters that can be specific for the group (inherited by configurations of that group)
    static const std::string KNOWN_GROUP_PARAMS[] = { "projection_evaluator",
                                                      "longest_valid_segment_fraction",
                                                      "enforce_joint_model_state_space" };

    // get parameters specific for the robot planning group
    std::map<std::string, std::string> specific_group_params;
    for (const std::string& k : KNOWN_GROUP_PARAMS)
    {
        std::string param_name{ group_name_ };
        param_name += "/";
        param_name += k;

        if (nh_.hasParam(param_name))
        {
            std::string value;
            if (nh_.getParam(param_name, value))
            {
                if (!value.empty())
                    specific_group_params[k] = value;
                continue;
            }

            double value_d;
            if (nh_.getParam(param_name, value_d))
            {
                // convert to string using no locale
                specific_group_params[k] = moveit::core::toString(value_d);
                continue;
            }

            int value_i;
            if (nh_.getParam(param_name, value_i))
            {
                specific_group_params[k] = std::to_string(value_i);
                continue;
            }

            bool value_b;
            if (nh_.getParam(param_name, value_b))
            {
                specific_group_params[k] = std::to_string(value_b);
                continue;
            }
        }

        // add default planner configuration
        planning_interface::PlannerConfigurationSettings default_pc;
        std::string default_planner_id;
        if (nh_.getParam(group_name_ + "/default_planner_config", default_planner_id))
        {
            if (!loadPlannerConfiguration(group_name_, default_planner_id, specific_group_params, default_pc))
                default_planner_id = "";
        }

        if (default_planner_id.empty())
        {
            default_pc.group = group_name_;
            default_pc.config = specific_group_params;
            default_pc.config["type"] = "wAstar";
        }

        default_pc.name = group_name_;  // this is the name of the default config
        pconfig[default_pc.name] = default_pc;

        // get parameters specific to each planner type
        XmlRpc::XmlRpcValue config_names;
        if (nh_.getParam(group_name_ + "/planner_configs", config_names))
        {
            if (config_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
            {
                ROS_ERROR_NAMED(LOGNAME,
                                "The planner_configs argument of a group configuration "
                                "should be an array of strings (for group '%s')",
                                group_name_.c_str());
                continue;
            }

            for (int j = 0; j < config_names.size(); ++j)  // NOLINT(modernize-loop-convert)
            {
                if (config_names[j].getType() != XmlRpc::XmlRpcValue::TypeString)
                {
                    ROS_ERROR_NAMED(LOGNAME, "Planner configuration names must be of type string (for group '%s')",
                                    group_name_.c_str());
                    continue;
                }

                const std::string planner_id = static_cast<std::string>(config_names[j]);

                planning_interface::PlannerConfigurationSettings pc;
                if (loadPlannerConfiguration(group_name_, planner_id, specific_group_params, pc))
                    pconfig[pc.name] = pc;
            }
        }
    }

    for (const std::pair<const std::string, planning_interface::PlannerConfigurationSettings>& config : pconfig)
    {
        ROS_DEBUG_STREAM_NAMED(LOGNAME, "Parameters for configuration '" << config.first << "'");

        for (const std::pair<const std::string, std::string>& parameters : config.second.config)
            ROS_DEBUG_STREAM_NAMED(LOGNAME, " - " << parameters.first << " = " << parameters.second);
    }

    setPlannerConfigurations(pconfig);
}

std::vector<std::string> SRMPInterface::getSubgroupNames(const std::string& group_name) {
    // Get the joint names, in order, per subgroup.
    auto joint_model_group_multi = robot_model_->getJointModelGroup(group_name);
    std::vector<std::string> move_group_joint_names;
    move_group_joint_names = joint_model_group_multi->getSubgroupNames();
    // Print the move groups found.
    for (const auto& subgroup_name : move_group_joint_names) {
        ROS_INFO_STREAM_NAMED(LOGNAME, "Subgroup (single agent): " << subgroup_name);
    }
    return move_group_joint_names;
}

void SRMPInterface::splitCompositeStartGoal(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                           const planning_interface::MotionPlanRequest& req,
                                           std::string group_name,
                                           const std::vector<std::string>& subgroups,
                                           std::vector<std::vector<double>>& start_states,
                                           std::vector<std::vector<double>>& goal_states) {
    // Get the joint names, in order, per subgroup.
    std::vector <std::vector<std::string>> move_group_joint_names;
    std::vector <std::string> move_group_multi_joint_names;
    for (const auto &subgroup_name: subgroups) {
        move_group_joint_names.push_back(robot_model_->getJointModelGroup(subgroup_name)->getActiveJointModelNames());
    }
    move_group_multi_joint_names = robot_model_->getJointModelGroup(group_name)->getActiveJointModelNames();
    std::cout << BOLDGREEN << "Joint names for each agent: " << move_group_joint_names << RESET << std::endl;
    std::cout << BOLDGREEN << "Joint names for all agents: " << move_group_multi_joint_names << RESET << std::endl;

    // Split the start and goal states to the agents.
    // Get the current robot state for the start state and joint names.
    std::vector <std::string> start_state_joint_names;
    robot_state::RobotState start_robot_state = planning_scene->getCurrentState();
    start_state_joint_names = start_robot_state.getVariableNames();

    std::vector <std::string> goal_state_joint_names;
    for (auto goal_constraint: req.goal_constraints) {
        for (auto joint_constraint: goal_constraint.joint_constraints) {
            goal_state_joint_names.push_back(joint_constraint.joint_name);
        }
    }

    std::cout << BOLDGREEN << "Start state joint names: " << start_state_joint_names << RESET << std::endl;
    std::cout << BOLDGREEN << "Goal state joint names: " << goal_state_joint_names << RESET << std::endl;

    // For each agent, go through their joints, get their name, and populate according to the request.
    for (int i = 0; i < subgroups.size(); i++) {
        std::vector<double> start_state;
        std::vector<double> goal_state;
        for (const auto& joint_name : move_group_joint_names.at(i)) {
            auto it_start = std::find(start_state_joint_names.begin(), start_state_joint_names.end(), joint_name);
            if (it_start != start_state_joint_names.end()) {
                int index = std::distance(start_state_joint_names.begin(), it_start);
                start_state.push_back(start_robot_state.getVariablePosition(joint_name));
            }
            else {
                std::cout << BOLDRED << "Joint name not found in start state." << RESET << std::endl;
                throw std::runtime_error("Joint name not found in start state.");
            }
            auto it_goal = std::find(goal_state_joint_names.begin(), goal_state_joint_names.end(), joint_name);
            if (it_goal != goal_state_joint_names.end()) {
                int index = std::distance(goal_state_joint_names.begin(), it_goal);
                goal_state.push_back(req.goal_constraints[0].joint_constraints[index].position);
            }
            else {
                std::cout << BOLDRED << "Joint name not found in goal state." << RESET << std::endl;
                throw std::runtime_error("Joint name not found in goal state.");
            }
        }
        // Add 0 or -1 in the time component (last) for the start or goal.
        start_state.push_back(0);
        goal_state.push_back(-1);

        start_states.push_back(start_state);
        goal_states.push_back(goal_state);
    }

    // Discretize, round, and print.
    for (int i = 0; i < subgroups.size(); i++) {
        std::cout << BOLDGREEN << "Start state for agent " << i << ": " << start_states[i] << RESET << std::endl;
        std::cout << BOLDGREEN << "Goal state for agent " << i << ": " << goal_states[i] << RESET << std::endl;
    }
}

bool SRMPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const planning_interface::MotionPlanRequest& req,
                          moveit_msgs::MotionPlanDetailedResponse& res) {
    // Set the planning scene in the PlanningWorld object.
    planning_world_->setPlanningSceneMoveIt(planning_scene);

    // Create a robot model.
    robot_model_ = planning_scene->getRobotModel();

    // Get the planner id from the request
    std::string planner_id = req.planner_id;
    if (planner_id.empty()) {
        planner_id = pconfig_.begin()->second.config.at("type");
    }
    ROS_INFO_STREAM_NAMED(LOGNAME, GREEN << "Planner id: " << planner_id << RESET);
    configuration_settings_.config["allowed_planning_time"] = std::to_string(req.allowed_planning_time);

    // Hack for checking if need to split the planning group into subgroups.
    if (ims::registered_multi_agent_planner_factories.find(planner_id) !=
        ims::registered_multi_agent_planner_factories.end()) {
        std::cout << BOLDGREEN << "Multi-agent planner." << RESET << std::endl;
        // Get group names.
        std::string group_name = req.group_name;
        std::vector <std::string> subgroups_names = getSubgroupNames(group_name);
        std::cout << BOLDGREEN << "Subgroup names: " << subgroups_names << RESET << std::endl;
        // Get a move group for the multi-robot.
        moveit::planning_interface::MoveGroupInterface move_group_multi = moveit::planning_interface::MoveGroupInterface(group_name);

        // Split the start and goal states to the agents.
        std::vector <std::vector<double>> agent_starts;
        std::vector <std::vector<double>> agent_goals;
        splitCompositeStartGoal(planning_scene, req, group_name, subgroups_names, agent_starts, agent_goals);

        // Create the planner.
        std::map<std::string, ims::PlannerContextValueType> planner_context;
        planner_context["planner_id"] = planner_id;
        for (const auto &param: configuration_settings_.config) {
            planner_context[param.first] = param.second;
        }

        planning_world_->makePlanner(subgroups_names, planner_context);

        // Solve the problem.
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
        std::vector <PathType> paths = planning_world_->plan(agent_starts, agent_goals);
        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
        double planning_time_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() / 1000.0;
        std::cout << "Planning took "
                  << planning_time_seconds << " seconds." << std::endl;

        // Process the result.
        if (paths.empty()) {
            res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
            return false;
        }

        MultiAgentPaths paths_map;
        for (int i = 0; i < subgroups_names.size(); i++) {
            paths_map[i] = paths[i];
        }

        // Shortcut the paths.
        std::string move_group_name = group_name;
        std::vector <std::string> robot_names = subgroups_names;
        planning_scene::PlanningScenePtr planning_scene_ptr = planning_scene->diff();
        std::unordered_map<int, std::string> robot_names_map;
        // Each robot's joints share a prefix. Find the prefix.
        for (size_t i = 0; i < robot_names.size(); i++) {
            std::string robot_name = robot_names[i];
            std::string prefix = robot_name.substr(0, robot_name.find("_"));
            robot_names_map[i] = prefix;
        }

        std::cout << robot_names_map << std::endl;
        MultiAgentPaths smoothed_paths;
        std::chrono::steady_clock::time_point begin_shortcut = std::chrono::steady_clock::now();
        ims::shortcutMultiAgentPathsIterative(paths_map, move_group_multi, planning_scene_ptr, robot_names_map, smoothed_paths, 1.0);
        paths_map = smoothed_paths;

        // The max path length.
        size_t T = 0;
        for (auto &path: paths_map) {
            if (path.second.size() > T) {
                T = path.second.size();
            }
        }
        // Create the agent_multi trajectory.
        PathType path_multi;
        for (int t = 0; t <= T - 1; t++) {
            StateType panda_multi_state;

            // Stack the states of all robots.
            for (int i = 0; i < paths_map.size(); i++) {
                int T_i = paths_map.at(i).size();
                int t_i = t;
                if (T_i <= t_i) {
                    t_i = T_i - 1;
                }

                StateType start_state_wo_time = paths_map.at(i)[t_i];
                start_state_wo_time.pop_back();  // Remove the time.

                // Add the state to the panda multi state.
                panda_multi_state.insert(panda_multi_state.end(), start_state_wo_time.begin(),
                                         start_state_wo_time.end());
            }
            path_multi.push_back(panda_multi_state);
        }

        std::chrono::steady_clock::time_point end_shortcut = std::chrono::steady_clock::now();

        // Convert to a moveit trajectory message.
        res.trajectory.resize(1);
        ims::profileTrajectory(path_multi.front(),
                               path_multi.back(),
                               path_multi,
                               move_group_multi,
                               res.trajectory[0],
                               req.max_velocity_scaling_factor,
                               req.max_acceleration_scaling_factor);

        // Timing.
//        res.processing_time.push_back(stats.time);
        res.processing_time.push_back(planning_time_seconds);
        res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
        res.group_name = req.group_name;

        return true;


    }

    else { // Single-agent planner.
        std::cout << BOLDGREEN << "Single agent planner." << RESET << std::endl;
        ROS_INFO_STREAM_NAMED(LOGNAME, GREEN << "Planner id: " << planner_id << RESET);
        // Get the group name.
        std::string group_name = req.group_name;
        // Create the planner context.
        std::map<std::string, ims::PlannerContextValueType> planner_context;
        planner_context["planner_id"] = planner_id;
        for (const auto &param : configuration_settings_.config) {
            planner_context[param.first] = param.second;
        }

        // Create the planner.
        planning_world_->makePlanner({group_name}, planner_context);
        // Get the start and goal states.
        moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel();
        moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model));
        *start_state = planning_scene->getCurrentState();
        const moveit::core::JointModelGroup *joint_model_group = start_state->getJointModelGroup(req.group_name);
        int dof = static_cast<int>(robot_model->getJointModelGroup(group_name)->getVariableCount());

        StateType start_joint_values(dof);
        start_state->copyJointGroupPositions(joint_model_group, start_joint_values);

        const std::vector <moveit_msgs::Constraints> &goal_constraints = req.goal_constraints;
        const std::vector <moveit_msgs::JointConstraint> &goal_joint_constraint = goal_constraints[0].joint_constraints;

        StateType goal_joint_values;
        goal_joint_values.reserve(goal_joint_constraint.size());
        for (const auto &constraint: goal_joint_constraint) {
            goal_joint_values.push_back(constraint.position);
        }

        // If the start and goal are the same then do nothing.
        if (start_joint_values == goal_joint_values) {
            ROS_INFO_STREAM_NAMED(LOGNAME, "Start and goal are the same");
            moveit_msgs::RobotTrajectory trajectory;
            ims::profileTrajectory(robot_model, req.group_name,
                                   {start_joint_values}, trajectory,
                                   req.max_velocity_scaling_factor,
                                   req.max_acceleration_scaling_factor);
            moveit::core::robotStateToRobotStateMsg(*start_state, res.trajectory_start);
            res.trajectory.push_back(trajectory);
            res.processing_time.push_back(0);
            res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
            res.group_name = req.group_name;
            return true;
        }


        // Solve.
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
        std::vector <StateType> path = planning_world_->plan(start_joint_values, goal_joint_values);
        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
        std::cout << "Planning took "
                  << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() / 1000.0
                  << " seconds." << std::endl;

        if (path.empty()) {
            res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
            return false;
        }

        std::vector<StateType> traj;
        for (auto& state : path) {
            traj.push_back(state);
            std::cout << state << std::endl;
        }
        moveit::planning_interface::MoveGroupInterface move_group = moveit::planning_interface::MoveGroupInterface(group_name);
        auto planning_scene_ptr = planning_scene->diff();
        ims::shortcutSmooth(traj, move_group, planning_scene_ptr, 0.5);

        moveit_msgs::RobotTrajectory trajectory;
        ims::profileTrajectory(robot_model, req.group_name, traj, trajectory,
                               req.max_velocity_scaling_factor,
                               req.max_acceleration_scaling_factor);
        // simplify the trajectory
        moveit::core::robotStateToRobotStateMsg(*start_state, res.trajectory_start);
        res.trajectory.push_back(trajectory);
        res.processing_time.push_back(std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() / 1000.0);
        res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
        res.group_name = req.group_name;
        return true;
    }

}

void SRMPInterface::clear() {

}
}  // namespace srmp_interface
