/*
 * Copyright (C) 2025, Itamar Mishani, Yorai Shaoul
 * 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   factories.hpp
 * \author Itamar Mishani (imishani@cmu.edu), Yoarai Shaoul (yorai@cmu.edu)
 * \date   2025-02-20
*/

#ifndef MANIPULATION_PLANNING_COMMON_FACTORIES_HPP
#define MANIPULATION_PLANNING_COMMON_FACTORIES_HPP

#include <variant>

#include <srmp_moveit_plugin/heuristics/manip_heuristics.hpp>
#include <moveit/planning_interface/planning_interface.h>
#include <search/planners/arastar.hpp>
#include <search/planners/mhastar.hpp>
#include <search/planners/multi_agent/ecbs.hpp>
#include <search/planners/multi_agent/eaecbs.hpp>
#include <search/planners/multi_agent/prioritized_planning.hpp>
#include <search/planners/parallel_search/pase.hpp>
#include "srmp_moveit_plugin/common/moveit_distance_field_wrapper.hpp"
#include "srmp_moveit_plugin/common/moveit_planning_scene_wrapper.hpp"
#include "srmp_moveit_plugin/common/moveit_scene_interface.hpp"
#include "manipulation_planning/action_space/manipulation_action_space.hpp"
#include "manipulation_planning/action_space/manipulation_edge_action_space.hpp"
#include "manipulation_planning/action_space/mramp_action_space.hpp"
#include "manipulation_planning/action_space/manipulation_experience_accelerated_constrained_action_space.hpp"

namespace ims {

// Public Types.
using PlannerContextValueType = std::variant<std::string, std::vector<std::string>>;

// Single robot cache variables.
struct SingleAgentPlanningConfiguration {
    // ====================
    // Identity variables (if all these are equal in a new request, we can use the cache with minimal updates).
    // ====================
    // The name of the planner.
    std::string planner_name;
    // The name of the heuristics to be used in the search.
    std::vector <std::string> heuristic_names;
    // The path to the motion primitives file.
    std::string path_mprim;
    // The configuration of the planner. E.g., heuristic weights, time limits, etc.
    std::map<std::string, PlannerContextValueType> planner_context;
    // The articulation group being planned for.
    std::string group_name;
    // ====================
    // Other variables.
    // ====================
    // The planner pointer.
    std::shared_ptr <ims::Planner> planner;
    // The heuristics (pointers) to be used in the search.
    std::vector<ims::BaseHeuristic *> heuristics;
    // The action space object.
    std::shared_ptr <ims::ActionSpace> action_space;
};

// Multi-robot cache variables.
struct MultiAgentPlanningConfiguration {
    // ====================
    // Identity variables (if all these are equal in a new request, we can use the cache with minimal updates).
    // ====================
    // The name of the planner.
    std::string planner_name;
    // The name of the heuristics to be used in the search. A list per agent.
    std::vector <std::vector<std::string>> agent_heuristics_names;
    // The path to the motion primitives file.
    std::vector <std::string> agent_mprim_paths;
    // The configuration of the planner.
    std::map <std::string, PlannerContextValueType> planner_context;
    // The group names of the agents.
    std::vector <std::string> agent_group_names;
    // ====================
    // Other variables.
    // ====================
    // The planner pointer.
    std::shared_ptr <ims::Planner> planner;
    // The heuristics (pointers) to be used in the search.
    std::vector <std::vector<ims::BaseHeuristic *>> agent_heuristics;
    // The action space object.
    std::vector <std::shared_ptr<ims::ActionSpace>> action_spaces;
};

// ===================
// Heuristic Factories
// ===================
inline auto MakeBFSHeuristic(std::string group_name,
                             const std::shared_ptr <MoveitInterface> &scene_interface) -> std::unique_ptr <BaseHeuristic> {

    std::cout << "Getting distance field" << std::endl;
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    ROS_INFO_STREAM("Got distance field");
    std::cout << "Got distance field" << std::endl;
    // try to get the group name from the planner configuration, otherwise throw an error
    if (group_name.empty()) {
        ROS_ERROR_STREAM("No group name specified for BFS heuristic");
        throw std::runtime_error("No group name specified for BFS heuristic");
    }
    return std::make_unique<BFSHeuristic>(df, group_name);
}

inline auto MakeJointAnglesHeuristic(std::string group_name,
                                     const std::shared_ptr <MoveitInterface> &scene_interface) -> std::unique_ptr <BaseHeuristic> {
    return std::make_unique<JointAnglesHeuristic>();
}

inline auto MakeEuclideanRemoveTimeHeuristic(std::string group_name,
                                             const std::shared_ptr <MoveitInterface> &scene_interface) -> std::unique_ptr <BaseHeuristic> {
    // Get the dofs of this group.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
            group_name)->getVariableCount());
    std::vector<bool> valid_mask(dof, true);
    valid_mask.back() = false;  // Time is not valid.
    std::cout << YELLOW << "Valid mask " << valid_mask << RESET << std::endl;
    return std::make_unique<EuclideanRemoveTimeHeuristic>();
}

inline auto MakeBFSRemoveTimeHeuristic(std::string group_name,
                                             const std::shared_ptr <MoveitInterface> &scene_interface) -> std::unique_ptr <BaseHeuristic> {
    std::cout << "Getting distance field" << std::endl;
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    std::cout << "Got distance field" << std::endl;
    // Try to get the group name from the planner configuration, otherwise throw an error
    if (group_name.empty()) {
        ROS_ERROR_STREAM("No group name specified for BFS heuristic");
        throw std::runtime_error("No group name specified for BFS heuristic");
    }
    return std::make_unique<BFSRemoveTimeHeuristic>(df, group_name);
}

// ===================
// Utility Functions.
// ===================
inline bool isSingleAgentCacheValid(const std::string &articulation_name,
                             const std::string &planner_name,
                             const std::vector <std::string> &heuristic_names,
                             const std::map<std::string, PlannerContextValueType> & planner_context,
                             const std::shared_ptr<MoveitInterface> & scene_interface,
                             const std::string &mprim_path,
                             const std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    std::cout << CYAN << "=====================\nChecking cache validity..." << RESET << std::endl;
    if (planner_cache == nullptr) {
        std::cout << CYAN << "No cache found. Creating new planner.\n=====================" << RESET << std::endl;
        return false;
    }
    bool is_valid = true;
    if (planner_cache->planner_name != planner_name){
        is_valid = false;
        std::cout << CYAN << "Planner name changed. Was: " << planner_cache->planner_name << ", Now: " << planner_name << RESET << std::endl;
    } else {
        std::cout << CYAN << "Planner name was: " << planner_cache->planner_name << ", now: " << planner_name << RESET << std::endl;
    }
    if (planner_cache->heuristic_names.size() != heuristic_names.size()) {
        is_valid = false;
        std::cout << CYAN << "Heuristic names changed. Was: " << planner_cache->heuristic_names << ", Now: " << heuristic_names << RESET << std::endl;
    }
    else {
        for (size_t i = 0; i < heuristic_names.size(); ++i) {
            is_valid &= planner_cache->heuristic_names.at(i) == heuristic_names.at(i);
        }
    }
    if (planner_cache->group_name != articulation_name) {
        is_valid = false;
        std::cout << CYAN << "Group name changed. Was: " << planner_cache->group_name << ", Now: " << articulation_name << RESET << std::endl;
    } else{
        std::cout << CYAN << "Group name was: " << planner_cache->group_name << ", now: " << articulation_name << RESET << std::endl;
    }

    for (auto param: planner_context) {
        bool is_param_valid_str = true;
        bool is_param_valid_vec = true;
        // Get from both maps, assuming string.
        std::string old_val, new_val;
        bool got_old = getValueFromVariantMap(planner_cache->planner_context, param.first, old_val);
        bool got_new = getValueFromVariantMap(planner_context, param.first, new_val);
        if (!got_old || !got_new || old_val != new_val) {
            is_param_valid_str = false;
            std::cout << CYAN << "Parameter " << param.first << " changed. Was: " << old_val << ", Now: " << new_val << RESET << std::endl;
        } else {
            std::cout << CYAN << "Parameter " << param.first << " was: " << old_val << ", now: " << new_val << RESET << std::endl;
        }
        // Get both assuming vector of strings.
        std::vector<std::string> old_vals, new_vals;
        got_old = getValueFromVariantMap(planner_cache->planner_context, param.first, old_vals);
        got_new = getValueFromVariantMap(planner_context, param.first, new_vals);
        if (!got_old || !got_new || old_vals != new_vals) {
            is_param_valid_vec = false;
            std::cout << CYAN << "Parameter " << param.first << " changed. Was: " << old_vals << ", Now: " << new_vals << RESET << std::endl;
        } else {
            std::cout << CYAN << "Parameter " << param.first << " was: " << old_vals << ", now: " << new_vals << RESET << std::endl;
        }
        is_valid &= is_param_valid_str || is_param_valid_vec;
    }

    if (is_valid){
        std::cout << CYAN << "Cache is valid.\n=====================" << RESET << std::endl;
    }
    else {
        std::cout << CYAN << "Cache is invalid.\n=====================" << RESET << std::endl;
    }
    return is_valid;
}

inline bool isMultiAgentCacheValid(const std::vector<std::string> & articulation_names,
                             const std::string &planner_name,
                             const std::vector<std::vector<std::string>> & heuristics_names,
                             const std::map<std::string, PlannerContextValueType> & planner_context,
                             const std::vector<std::shared_ptr<MoveitInterface>> & scene_interfaces,
                             const std::vector <std::string> & mprim_paths,
                             const std::shared_ptr<MultiAgentPlanningConfiguration> &planner_cache) {
    std::cout << CYAN << "=====================\nChecking cache validity..." << RESET << std::endl;
    if (planner_cache == nullptr) {
        std::cout << CYAN << "No cache found. Creating new planner.\n=====================" << RESET << std::endl;
        return false;
    }
    bool is_valid = true;
    if (planner_cache->planner_name != planner_name){
        is_valid = false;
        std::cout << CYAN << "Planner name changed. Was: " << planner_cache->planner_name << ", Now: " << planner_name << RESET << std::endl;
    } else {
        std::cout << CYAN << "Planner name was: " << planner_cache->planner_name << ", now: " << planner_name << RESET << std::endl;
    }
    if (planner_cache->agent_heuristics_names.size() != heuristics_names.size()) {
        is_valid = false;
        std::cout << CYAN << "Heuristic names changed. Was: " << planner_cache->agent_heuristics_names << ", Now: " << heuristics_names << RESET << std::endl;
    }
    else {
        for (size_t i = 0; i < heuristics_names.size(); ++i) {
            is_valid &= planner_cache->agent_heuristics_names.at(i) == heuristics_names.at(i);
            if (planner_cache->agent_heuristics_names.at(i) != heuristics_names.at(i)) {
                is_valid = false;
                std::cout << CYAN << "Heuristic names changed. Was: " << planner_cache->agent_heuristics_names.at(i) << ", Now: " << heuristics_names.at(i) << RESET << std::endl;
            } else{
                std::cout << CYAN << "Heuristic names was: " << planner_cache->agent_heuristics_names.at(i) << ", now: " << heuristics_names.at(i) << RESET << std::endl;
            }
        }
    }
    for (size_t i = 0; i < articulation_names.size(); ++i) {
        if (planner_cache->agent_group_names.at(i) != articulation_names.at(i)) {
            is_valid = false;
            std::cout << CYAN << "Group name changed. Was: " << planner_cache->agent_group_names.at(i) << ", Now: " << articulation_names.at(i) << RESET << std::endl;
        } else{
            std::cout << CYAN << "Group name was: " << planner_cache->agent_group_names.at(i) << ", now: " << articulation_names.at(i) << RESET << std::endl;
        }
    }

    for (auto param: planner_context) {
        bool is_param_valid_str = true;
        bool is_param_valid_vec = true;
        // Get from both maps, assuming string.
        std::string old_val, new_val;
        bool got_old = getValueFromVariantMap(planner_cache->planner_context, param.first, old_val);
        bool got_new = getValueFromVariantMap(planner_context, param.first, new_val);
        if (!got_old || !got_new || old_val != new_val) {
            is_param_valid_str = false;
            std::cout << CYAN << "Parameter " << param.first << " changed. Was: " << old_val << ", Now: " << new_val << RESET << std::endl;
        } else {
            std::cout << CYAN << "Parameter " << param.first << " was: " << old_val << ", now: " << new_val << RESET << std::endl;
        }
        // Get both assuming vector of strings.
        std::vector<std::string> old_vals, new_vals;
        got_old = getValueFromVariantMap(planner_cache->planner_context, param.first, old_vals);
        got_new = getValueFromVariantMap(planner_context, param.first, new_vals);
        if (!got_old || !got_new || old_vals != new_vals) {
            is_param_valid_vec = false;
            std::cout << CYAN << "Parameter " << param.first << " changed. Was: " << old_vals << ", Now: " << new_vals << RESET << std::endl;
        } else {
            std::cout << CYAN << "Parameter " << param.first << " was: " << old_vals << ", now: " << new_vals << RESET << std::endl;
        }
        is_valid &= is_param_valid_str || is_param_valid_vec;
    }

    if (is_valid){
        std::cout << CYAN << "Cache is valid.\n=====================" << RESET << std::endl;
    }
    else {
        std::cout << CYAN << "Cache is invalid.\n=====================" << RESET << std::endl;
    }
    return is_valid;
}

inline std::string getDefaultMprimPath(int dof, bool is_timed = false) {
    auto full_path = ros::package::getPath("manipulation_planning");
    std::string path_mprim;
    if (is_timed) {
        return full_path + "/config/manip_" + std::to_string(dof) + "dof_timed_mprim.yaml";
    }
    else {
        return full_path + "/config/manip_" + std::to_string(dof) + "dof_mprim.yaml";
    }
}

// ===================
// Heuristic Factories
// ===================
using HeuristicFactory = std::function<
    std::unique_ptr<ims::BaseHeuristic>(std::string
            group_name,
            const std::shared_ptr <MoveitInterface> &scene_interface
    )>;
static std::map<std::string, HeuristicFactory> registered_heuristic_factories =
    {
        {"bfs", MakeBFSHeuristic},
        {"joint_euclidean", MakeJointAnglesHeuristic},
        {"joint_euclidean_remove_time", MakeEuclideanRemoveTimeHeuristic}
    };

static std::map<std::string, HeuristicFactory> registered_timed_heuristic_factories =
    {
        {"joint_euclidean", MakeEuclideanRemoveTimeHeuristic},
        {"bfs", MakeBFSRemoveTimeHeuristic}
    };

// ===================
// Planner Factories
// ===================
/***
 * @brief This method creates a new A* planner from scratch or reuses one from cache.
 * @param planner_parameters Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return
 */
inline std::shared_ptr<Planner> makeOrUpdateAStarPlanner(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> & planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "Astar";
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::string> heuristic_names = {heuristic_name};
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isSingleAgentCacheValid(articulation_name,
                                                  planner_name,
                                                  heuristic_names,
                                                  planner_context,
                                                  scene_interface,
                                                  mprim_path,
                                                  planner_cache);

    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for A* planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        planner_cache->action_space->resetPlanningData();
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for A* planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of the robot.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
    // Create the scene interface. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for A* planner." << RESET << std::endl;
        // Get the default path.
        mprim_path = getDefaultMprimPath(dof);
        std::cout << BOLDGREEN << "Getting default mprim path for " << dof << "dof manipulator: " << mprim_path << RESET << std::endl;
    }

    // Create the action type.
    std::shared_ptr<ManipulationType> action_type = std::make_shared<ManipulationType>(mprim_path);
    // Get resolution from parameters.
    std::string resolution;
    if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
        throw std::runtime_error("No resolution specified for A* planner.");
    }
    std::vector<double> discretization(dof, std::stod(resolution));
    ims::deg2rad(discretization);
    action_type->Discretization(discretization);
    // Create a snap heuristic.
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    ims::BFSHeuristic* snap_heuristic = new ims::BFSHeuristic(df, articulation_name);
    // NOTE(yoraish): This will be set when the goal is available: snap_heuristic->setGoalStateValue(goal_state);
    // Create the action space.
    std::shared_ptr<ActionSpace> action_space = std::make_shared<ManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interface),
                                                                                          *action_type,
                                                                                          snap_heuristic);

    // Create the heuristics.
    std::vector<BaseHeuristic *> heuristics;
    for (const auto &heuristic_name : heuristic_names) {
        std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
        auto heuristic_factory = registered_heuristic_factories.find(heuristic_name);
        if (heuristic_factory == registered_heuristic_factories.end()) {
            ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
            throw std::runtime_error("Could not find heuristic factory");
        }
        heuristics.push_back(heuristic_factory->second(articulation_name, scene_interface).release());
        std::cout << BOLDGREEN << "OK. A* heuristic created." << RESET << std::endl;
    }

    // Create the planner.
    auto aStarParams = AStarParams(heuristics.at(0)); // Check for max time.
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    aStarParams.time_limit_ = std::stod(time_limit);

    planner_cache = std::make_shared<SingleAgentPlanningConfiguration>();
    std::shared_ptr<AStar> planner = std::make_shared<AStar>(aStarParams);
    planner_cache->planner = planner;
    planner_cache->heuristics = heuristics;
    planner_cache->action_space = action_space;
    planner_cache->planner_name = planner_name;
    planner_cache->heuristic_names = heuristic_names;
    planner_cache->path_mprim = mprim_path;
    planner_cache->planner_context = planner_context;
    planner_cache->group_name = articulation_name;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "A* planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}


/***
 * @brief This method creates a new wA* planner from scratch or reuses one from cache.
 * @param planner_parameters Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return
 */
inline std::shared_ptr<Planner> makeOrUpdateWAStarPlanner(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "wAstar";
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::string> heuristic_names = {heuristic_name};
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isSingleAgentCacheValid(articulation_name,
                                                  planner_name,
                                                  heuristic_names,
                                                  planner_context,
                                                  scene_interface,
                                                  mprim_path,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for wA* planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        planner_cache->action_space->resetPlanningData();
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for wA* planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of the robot.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
    // Create the scene interface. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for wA* planner." << RESET << std::endl;
        // Get the default path.
        mprim_path = getDefaultMprimPath(dof);
        std::cout << BOLDGREEN << "Getting default mprim path for " << dof << "dof manipulator: " << mprim_path << RESET << std::endl;
    }


    // Create the action type.
    std::shared_ptr<ManipulationType> action_type = std::make_shared<ManipulationType>(mprim_path);
    // Get resolution from parameters.
    std::string resolution;
    if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
        throw std::runtime_error("No resolution specified for wA* planner.");
    }
    std::vector<double> discretization(dof, std::stod(resolution));
    ims::deg2rad(discretization);
    action_type->Discretization(discretization);
    // Create a snap heuristic.
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    ims::BFSHeuristic* snap_heuristic = new ims::BFSHeuristic(df, articulation_name);
    // NOTE(yoraish): This will be set when the goal is available: snap_heuristic->setGoalStateValue(goal_state);
    // Create the action space.
    std::shared_ptr<ActionSpace> action_space = std::make_shared<ManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interface),
                                                                                          *action_type,
                                                                                          snap_heuristic);

    // Create the heuristics.
    std::vector<BaseHeuristic *> heuristics;
    for (const auto &heuristic_name : heuristic_names) {
        std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
        auto heuristic_factory = registered_heuristic_factories.find(heuristic_name);
        if (heuristic_factory == registered_heuristic_factories.end()) {
            ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
            throw std::runtime_error("Could not find heuristic factory");
        }
        heuristics.push_back(heuristic_factory->second(articulation_name, scene_interface).release());
        std::cout << BOLDGREEN << "OK. wA* heuristic created." << RESET << std::endl;
    }
    // Create the planner.
    // Check if a weight is specified.
    std::string weight = "50";
    getValueFromVariantMap(planner_context, "weight", weight);
    auto params_wastar = wAStarParams(heuristics.at(0), std::stod(weight));

    // Check for max time.
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params_wastar.time_limit_ = std::stod(time_limit);


    planner_cache = std::make_shared<SingleAgentPlanningConfiguration>();
    std::shared_ptr<wAStar> planner = std::make_shared<wAStar>(params_wastar);
    planner_cache->planner = planner;
    planner_cache->heuristics = heuristics;
    planner_cache->action_space = action_space;
    planner_cache->planner_name = planner_name;
    planner_cache->heuristic_names = heuristic_names;
    planner_cache->path_mprim = mprim_path;
    planner_cache->planner_context = planner_context;
    planner_cache->group_name = articulation_name;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "wA* planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}

/***
 * @brief This method creates a new ARA* planner from scratch or reuses one from cache.
 * @param planner_parameters Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return
 */
inline std::shared_ptr<Planner> makeOrUpdateARAStarPlanner(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "ARAstar";
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::string> heuristic_names = {heuristic_name};
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isSingleAgentCacheValid(articulation_name,
                                                  planner_name,
                                                  heuristic_names,
                                                  planner_context,
                                                  scene_interface,
                                                  mprim_path,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for ARA* planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        planner_cache->action_space->resetPlanningData();
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for ARA* planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of the robot.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
    // Create the scene interface. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for ARA* planner." << RESET << std::endl;
        // Get the default path.
        mprim_path = getDefaultMprimPath(dof);
        std::cout << BOLDGREEN << "Getting default mprim path for " << dof << "dof manipulator: " << mprim_path << RESET << std::endl;
    }


    // Create the action type.
    std::shared_ptr<ManipulationType> action_type = std::make_shared<ManipulationType>(mprim_path);
    // Get resolution from parameters.
    std::string resolution;
    if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
        throw std::runtime_error("No resolution specified for ARA* planner.");
    }
    std::vector<double> discretization(dof, std::stod(resolution));
    ims::deg2rad(discretization);
    action_type->Discretization(discretization);
    // Create a snap heuristic.
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    ims::BFSHeuristic* snap_heuristic = new ims::BFSHeuristic(df, articulation_name);
    // NOTE(yoraish): This will be set when the goal is available: snap_heuristic->setGoalStateValue(goal_state);
    // Create the action space.
    std::shared_ptr<ActionSpace> action_space = std::make_shared<ManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interface),
                                                                                          *action_type,
                                                                                          snap_heuristic);

    // Create the heuristics.
    std::vector<BaseHeuristic *> heuristics;
    for (const auto &heuristic_name : heuristic_names) {
        std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
        auto heuristic_factory = registered_heuristic_factories.find(heuristic_name);
        if (heuristic_factory == registered_heuristic_factories.end()) {
            ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
            throw std::runtime_error("Could not find heuristic factory");
        }
        heuristics.push_back(heuristic_factory->second(articulation_name, scene_interface).release());
        std::cout << BOLDGREEN << "OK. ARA* heuristic created." << RESET << std::endl;
    }
    // Create the planner.

    std::string initial_weight_str = "100.0";
    std::string weight_delta_str = "10.0";
    std::string final_weight_str = "1.0";
    getValueFromVariantMap(planner_context, "initial_weight", initial_weight_str);
    getValueFromVariantMap(planner_context, "weight_delta", weight_delta_str);
    getValueFromVariantMap(planner_context, "final_weight", final_weight_str);

    ims::ARAStarParams params(heuristics.at(0),
                              std::stod(initial_weight_str),
                              std::stod(weight_delta_str),
                              std::stod(final_weight_str));

    // Check for max time.
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params.time_limit_ = std::stod(time_limit);
    params.ara_time_limit = std::stod(time_limit);

    planner_cache = std::make_shared<SingleAgentPlanningConfiguration>();
    std::shared_ptr<ARAStar> planner = std::make_shared<ARAStar>(params);
    planner_cache->planner = planner;
    planner_cache->heuristics = heuristics;
    planner_cache->action_space = action_space;
    planner_cache->planner_name = planner_name;
    planner_cache->heuristic_names = heuristic_names;
    planner_cache->path_mprim = mprim_path;
    planner_cache->planner_context = planner_context;
    planner_cache->group_name = articulation_name;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "ARA* planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}

/***
 * @brief This method creates a new MHA* planner from scratch or reuses one from cache.
 * @param planner_parameters Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return
 */
inline std::shared_ptr<Planner> makeOrUpdateMHAStarPlanner(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "MHAstar";
    // Get the heuristic.
    std::string anchor_heuristic_name = "joint_euclidean";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", anchor_heuristic_name);
    std::vector<std::string> inadmissible_heuristic_names = {"bfs"};  // Default heuristic.
    getValueFromVariantMap(planner_context, "inadmissible_heuristics", inadmissible_heuristic_names);
    std::vector<std::string> heuristic_names = {anchor_heuristic_name};
    heuristic_names.insert(heuristic_names.end(), inadmissible_heuristic_names.begin(), inadmissible_heuristic_names.end());
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isSingleAgentCacheValid(articulation_name,
                                                  planner_name,
                                                  heuristic_names,
                                                  planner_context,
                                                  scene_interface,
                                                  mprim_path,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for MHA* planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        planner_cache->action_space->resetPlanningData();
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for MHA* planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of the robot.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
    // Create the scene interface. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for MHA* planner." << RESET << std::endl;
        // Get the default path.
        mprim_path = getDefaultMprimPath(dof);
        std::cout << BOLDGREEN << "Getting default mprim path for " << dof << "dof manipulator: " << mprim_path << RESET << std::endl;
    }

    // Create the action type.
    std::shared_ptr<ManipulationType> action_type = std::make_shared<ManipulationType>(mprim_path);
    // Get resolution from parameters.
    std::string resolution;
    if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
        throw std::runtime_error("No resolution specified for MHA* planner.");
    }
    std::vector<double> discretization(dof, std::stod(resolution));
    ims::deg2rad(discretization);
    action_type->Discretization(discretization);
    // Create a snap heuristic.
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    ims::BFSHeuristic* snap_heuristic = new ims::BFSHeuristic(df, articulation_name);
    // NOTE(yoraish): This will be set when the goal is available: snap_heuristic->setGoalStateValue(goal_state);
    // Create the action space.
    std::shared_ptr<ActionSpace> action_space = std::make_shared<ManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interface),
                                                                                          *action_type,
                                                                                          snap_heuristic);
    // Create the heuristics.
    std::vector<BaseHeuristic *> heuristics;
    for (const auto &heuristic_name : heuristic_names) {
        std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
        auto heuristic_factory = registered_heuristic_factories.find(heuristic_name);
        if (heuristic_factory == registered_heuristic_factories.end()) {
            ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
            throw std::runtime_error("Could not find heuristic factory");
        }
        heuristics.push_back(heuristic_factory->second(articulation_name, scene_interface).release());
        std::cout << BOLDGREEN << "OK. MHA* heuristics created." << RESET << std::endl;
    }
    if (heuristics.size() < 2) {
        ROS_ERROR_STREAM("MHAstar requires at least two heuristics");
        throw std::runtime_error("MHAstar requires at least two heuristics");
    }
    BaseHeuristic *anchor_heuristic = heuristics.at(0);
    std::vector < BaseHeuristic * > inadmissible_heuristics;
    for (size_t i = 1; i < heuristics.size(); ++i) {
        inadmissible_heuristics.push_back(heuristics.at(i));
    }
    // Get w1 and w2.
    std::string w1_str = "20";
    std::string w2_str = "5";
    getValueFromVariantMap(planner_context, "w1", w1_str);
    getValueFromVariantMap(planner_context, "w2", w2_str);


    ims::MHAStarParams params(anchor_heuristic, inadmissible_heuristics, std::stod(w1_str), std::stod(w2_str));
    // Check for max time.
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params.time_limit_ = std::stod(time_limit);
    std::shared_ptr<MHAStar> planner = std::make_shared<MHAStar>(params);

    planner_cache = std::make_shared<SingleAgentPlanningConfiguration>();
    planner_cache->planner = planner;
    planner_cache->heuristics = heuristics;
    planner_cache->action_space = action_space;
    planner_cache->planner_name = planner_name;
    planner_cache->heuristic_names = heuristic_names;
    planner_cache->path_mprim = mprim_path;
    planner_cache->planner_context = planner_context;
    planner_cache->group_name = articulation_name;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "MHA* planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}

/***
 * @brief This method creates a new wA* planner from scratch or reuses one from cache.
 * @param planner_parameters Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return
 */
inline std::shared_ptr<Planner> makeOrUpdateWPASEPlanner(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache) {
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "wPASE";
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::string> heuristic_names = {heuristic_name};
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isSingleAgentCacheValid(articulation_name,
                                                  planner_name,
                                                  heuristic_names,
                                                  planner_context,
                                                  scene_interface,
                                                  mprim_path,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for wPA*SE planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        planner_cache->action_space->resetPlanningData();
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for wPA*SE planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of the robot.
    auto planning_scene_wrapper = scene_interface->getPlanningSceneMoveit();
    auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
    int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
    // Create the scene interface. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for wPA*SE planner." << RESET << std::endl;
        // Get the default path.
        mprim_path = getDefaultMprimPath(dof);
        std::cout << BOLDGREEN << "Getting default mprim path for " << dof << "dof manipulator: " << mprim_path << RESET << std::endl;
    }


    // Create the action type.
    std::shared_ptr<ManipulationType> action_type = std::make_shared<ManipulationType>(mprim_path);
    // Get resolution from parameters.
    std::string resolution;
    if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
        throw std::runtime_error("No resolution specified for wPA*SE planner.");
    }
    std::vector<double> discretization(dof, std::stod(resolution));
    ims::deg2rad(discretization);
    action_type->Discretization(discretization);
    // Create a snap heuristic.
    auto df_wrapper = scene_interface->getDistanceFieldMoveItInterface();
    auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
    auto snap_heuristic = std::make_shared<ims::BFSHeuristic>(df, articulation_name);
    // NOTE(yoraish): This will be set when the goal is available: snap_heuristic->setGoalStateValue(goal_state);
    // Create the action space.
    std::shared_ptr<ManipulationEdgeActionSpace> action_space = std::make_shared<ManipulationEdgeActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interface),
                                                                                          *action_type,
                                                                                          snap_heuristic);

    // Create the heuristics.
    std::vector<BaseHeuristic *> heuristics;
    for (const auto &heuristic_name : heuristic_names) {
        std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
        auto heuristic_factory = registered_heuristic_factories.find(heuristic_name);
        if (heuristic_factory == registered_heuristic_factories.end()) {
            ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
            throw std::runtime_error("Could not find heuristic factory");
        }
        heuristics.push_back(heuristic_factory->second(articulation_name, scene_interface).release());
        std::cout << BOLDGREEN << "OK. wPA*SE heuristic created." << RESET << std::endl;
    }

    // Create the i_heuristic.
    auto i_heuristic = std::make_shared<ims::JointAnglesHeuristic>();

    // Create the planner.
    // Check if a weight is specified.
    std::string weight = "50";
    getValueFromVariantMap(planner_context, "weight", weight);
    // Check if weight_i is specified.
    std::string weight_i = "100";
    getValueFromVariantMap(planner_context, "weight_i", weight_i);
    // Check if num_threads is specified.
    std::string num_threads = "4";
    getValueFromVariantMap(planner_context, "num_threads", num_threads);
    ims::ParallelSearchParams params(std::shared_ptr<BaseHeuristic>(heuristics.at(0)),
                                     i_heuristic,
                                     std::stod(num_threads),
                                     std::stod(weight),
                                     std::stod(weight_i));
    // Check for max time.
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params.time_limit_ = std::stod(time_limit);


    planner_cache = std::make_shared<SingleAgentPlanningConfiguration>();
    std::shared_ptr<Pase> planner = std::make_shared<Pase>(params);
    planner_cache->planner = planner;
    planner_cache->heuristics = heuristics;
    planner_cache->action_space = action_space;
    planner_cache->planner_name = planner_name;
    planner_cache->heuristic_names = heuristic_names;
    planner_cache->path_mprim = mprim_path;
    planner_cache->planner_context = planner_context;
    planner_cache->group_name = articulation_name;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "wPA*SE planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}


using PlannerFactory = std::function<
    std::shared_ptr<ims::Planner>(
            const std::string &articulation_name,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::shared_ptr<MoveitInterface> &scene_interface,
            std::shared_ptr<SingleAgentPlanningConfiguration> &planner_cache)
    >;


/***
 * @brief This method creates a new ECBS planner from scratch or reuses one from cache.
 * @param articulation_names The names of the articulation groups being planned for.
 * @param planner_context Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param scene_interfaces The scene interfaces for each of the agents.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return The ECBS planner.
 */
inline std::shared_ptr<Planner> makeOrUpdateECBSPlanner(
            const std::vector<std::string> &articulation_names,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::vector<std::shared_ptr<MoveitInterface>> &scene_interfaces,
            std::shared_ptr<MultiAgentPlanningConfiguration> &planner_cache) {
    std::cout << BOLDGREEN << "Creating ECBS." << RESET << std::endl;
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "ECBS";
    int num_agents = articulation_names.size();
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::vector<std::string>> heuristics_names(num_agents, {heuristic_name});
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);
    std::vector<std::string> mprim_paths(articulation_names.size(), mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isMultiAgentCacheValid(articulation_names,
                                                  planner_name,
                                                  heuristics_names,
                                                  planner_context,
                                                  scene_interfaces,
                                                  mprim_paths,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for ECBS planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        for (auto action_space: planner_cache->action_spaces) {
            action_space->resetPlanningData();
        }
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for ECBS planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of each of the agents.
    std::vector<int> agent_dofs;
    for (int i = 0; i < num_agents; ++i) {
        std::string articulation_name = articulation_names.at(i);
        auto planning_scene_wrapper = scene_interfaces.at(i)->getPlanningSceneMoveit();
        auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
        int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
        agent_dofs.push_back(dof);
    }
    std::cout << BOLDGREEN << "Agent DoFs: " << agent_dofs << RESET << std::endl;
    // Create the scene interfaces. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for ECBS planner." << RESET << std::endl;
        // Get the default path.
        for (int i = 0; i < num_agents; ++i) {
            mprim_paths.at(i) = getDefaultMprimPath(agent_dofs.at(i), true);
            std::cout << BOLDGREEN << "Getting default mprim path for " << agent_dofs.at(i) << "dof manipulator: " << mprim_paths.at(i) << RESET << std::endl;
        }
    }


    // Create the action types.
    std::vector<std::shared_ptr<MrampManipulationActionType>> action_types;// = std::make_shared<ManipulationType>(mprim_path);
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionType> action_type = std::make_shared<MrampManipulationActionType>(mprim_paths.at(i));
        // Get resolution from parameters.
        std::string resolution;
        if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
            throw std::runtime_error("No resolution specified for ECBS planner.");
        }
        std::vector<double> discretization(agent_dofs.at(i), std::stod(resolution));
        ims::deg2rad(discretization);
        discretization.push_back(1);  // Time is resolution 1.
        std::cout << BOLDGREEN << "Discretization for agent " << i << ": " << discretization << RESET << std::endl;
        action_type->Discretization(discretization);
        action_types.push_back(action_type);
    }

    // Create the snap heuristics.
    std::vector<std::shared_ptr<distance_field::PropagationDistanceField>> dfs;
    std::vector<ims::BFSRemoveTimeHeuristic*> snap_heuristics;
    for (int i = 0; i < num_agents; ++i) {
        auto df_wrapper = scene_interfaces.at(i)->getDistanceFieldMoveItInterface();
        auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
        dfs.push_back(df);
        ims::BFSRemoveTimeHeuristic* snap_heuristic = new ims::BFSRemoveTimeHeuristic(df, articulation_names.at(i));
        snap_heuristics.push_back(snap_heuristic);
    }
    // Create the action spaces.
    std::cout << GREEN << "Creating action spaces..." << RESET << std::endl;
    std::vector<std::shared_ptr<ActionSpace>> action_spaces;
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionSpace> action_space =
                std::make_shared<MrampManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interfaces.at(i)),
                                                          *action_types.at(i),
                                                          snap_heuristics.at(i));
        std::vector<bool> state_angle_cont_mask(agent_dofs.at(i), false);
        action_space->setStateAnglesContinuousMask(state_angle_cont_mask);
        action_spaces.push_back(action_space);
    }
    std::cout << BOLDGREEN << "Action spaces created." << RESET << std::endl;

    // Create the heuristics. Each agent has its own list of heuristics, normally only one in each.
    std::vector<std::vector<BaseHeuristic *>> heuristics;
    for (int i = 0; i < num_agents; ++i) {
        std::vector<BaseHeuristic *> agent_heuristics;
        for (const auto &heuristic_name : heuristics_names.at(i)) {
            std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
            auto heuristic_factory = registered_timed_heuristic_factories.find(heuristic_name);
            if (heuristic_factory == registered_timed_heuristic_factories.end()) {
                ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
                throw std::runtime_error("Could not find heuristic factory");
            }
            agent_heuristics.push_back(heuristic_factory->second(articulation_names.at(i), scene_interfaces.at(i)).release());
            std::cout << BOLDGREEN << "OK. ECBS heuristic created for agent " << i << ": " << heuristic_name << RESET << std::endl;
        }
        heuristics.push_back(agent_heuristics);
    }

    // Create the planner.
    ECBSParams ecbs_params;
    // Setting params_ecbs.weight_low_level_heuristic;
    std::string weight_low_level_heuristic_str = "55";
    getValueFromVariantMap(planner_context, "w_low_level", weight_low_level_heuristic_str);
    ecbs_params.weight_low_level_heuristic = std::stod(weight_low_level_heuristic_str);
    std::string weight_focal_high_level_str = "1.3";
    getValueFromVariantMap(planner_context, "w_focal_high_level", weight_focal_high_level_str);
    ecbs_params.high_level_focal_suboptimality = std::stod(weight_focal_high_level_str);
    std::string weight_focal_low_level_str = "1.3";
    getValueFromVariantMap(planner_context, "w_focal_low_level", weight_focal_low_level_str);
    ecbs_params.low_level_focal_suboptimality = std::stod(weight_focal_low_level_str);
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    ecbs_params.time_limit_ = std::stod(time_limit);
    ecbs_params.is_root_trick = true;

    ecbs_params.low_level_heuristic_ptrs.clear();
    for (int i = 0; i < num_agents; ++i) {
        for (auto heuristic: heuristics.at(i)) {
            ecbs_params.low_level_heuristic_ptrs.push_back(heuristic);
        }
    }

    // Set the conflict and constraints types.
//    ecbs_params.conflict_type_to_constraint_types = {
//            {ims::ConflictType::VERTEX_POINT3D, {ims::ConstraintType::VERTEX_POINT3D}},
//            {ims::ConflictType::EDGE_POINT3D,   {ims::ConstraintType::EDGE_POINT3D}}
//    };

    planner_cache = std::make_shared<MultiAgentPlanningConfiguration>();
    std::shared_ptr<ECBS> planner = std::make_shared<ECBS>(ecbs_params);
    planner_cache->planner = planner;
    planner_cache->agent_heuristics = heuristics;
    planner_cache->action_spaces = action_spaces;
    planner_cache->planner_name = planner_name;
    planner_cache->agent_heuristics_names = heuristics_names;
    planner_cache->agent_mprim_paths = mprim_paths;
    planner_cache->planner_context = planner_context;
    planner_cache->agent_group_names = articulation_names;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "ECBS planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}


/***
 * @brief This method creates a new xECBS planner from scratch or reuses one from cache.
 * @param articulation_names The names of the articulation groups being planned for.
 * @param planner_context Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param scene_interfaces The scene interfaces for each of the agents.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return The xECBS planner.
 */
inline std::shared_ptr<Planner> makeOrUpdateXECBSPlanner(
            const std::vector<std::string> &articulation_names,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::vector<std::shared_ptr<MoveitInterface>> &scene_interfaces,
            std::shared_ptr<MultiAgentPlanningConfiguration> &planner_cache) {
    std::cout << BOLDGREEN << "Creating xECBS." << RESET << std::endl;
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "xECBS";
    int num_agents = articulation_names.size();
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::vector<std::string>> heuristics_names(num_agents, {heuristic_name});
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);
    std::vector<std::string> mprim_paths(articulation_names.size(), mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isMultiAgentCacheValid(articulation_names,
                                                  planner_name,
                                                  heuristics_names,
                                                  planner_context,
                                                  scene_interfaces,
                                                  mprim_paths,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for xECBS planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        for (auto action_space: planner_cache->action_spaces) {
            action_space->resetPlanningData();
        }
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for xECBS planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of each of the agents.
    std::vector<int> agent_dofs;
    for (int i = 0; i < num_agents; ++i) {
        std::string articulation_name = articulation_names.at(i);
        auto planning_scene_wrapper = scene_interfaces.at(i)->getPlanningSceneMoveit();
        auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
        int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
        agent_dofs.push_back(dof);
    }
    std::cout << BOLDGREEN << "Agent DoFs: " << agent_dofs << RESET << std::endl;
    // Create the scene interfaces. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for xECBS planner." << RESET << std::endl;
        // Get the default path.
        for (int i = 0; i < num_agents; ++i) {
            mprim_paths.at(i) = getDefaultMprimPath(agent_dofs.at(i), true);
            std::cout << BOLDGREEN << "Getting default mprim path for " << agent_dofs.at(i) << "dof manipulator: " << mprim_paths.at(i) << RESET << std::endl;
        }
    }


    // Create the action types.
    std::vector<std::shared_ptr<MrampManipulationActionType>> action_types;// = std::make_shared<ManipulationType>(mprim_path);
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionType> action_type = std::make_shared<MrampManipulationActionType>(mprim_paths.at(i));
        // Get resolution from parameters.
        std::string resolution;
        if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
            throw std::runtime_error("No resolution specified for xECBS planner.");
        }
        std::vector<double> discretization(agent_dofs.at(i), std::stod(resolution));
        ims::deg2rad(discretization);
        discretization.push_back(1);  // Time is resolution 1.
        std::cout << BOLDGREEN << "Discretization for agent " << i << ": " << discretization << RESET << std::endl;
        action_type->Discretization(discretization);
        action_types.push_back(action_type);
    }

    // Create the snap heuristics.
    std::vector<std::shared_ptr<distance_field::PropagationDistanceField>> dfs;
    std::vector<ims::BFSRemoveTimeHeuristic*> snap_heuristics;
    for (int i = 0; i < num_agents; ++i) {
        auto df_wrapper = scene_interfaces.at(i)->getDistanceFieldMoveItInterface();
        auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
        dfs.push_back(df);
        ims::BFSRemoveTimeHeuristic* snap_heuristic = new ims::BFSRemoveTimeHeuristic(df, articulation_names.at(i));
//        snap_heuristic->setDistanceField(df);
        snap_heuristics.push_back(snap_heuristic);
    }
    // Create the action spaces.
    std::cout << GREEN << "Creating action spaces..." << RESET << std::endl;
    std::vector<std::shared_ptr<ActionSpace>> action_spaces;
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionSpace> action_space =
                std::make_shared<MrampManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interfaces.at(i)),
                                                          *action_types.at(i),
                                                          snap_heuristics.at(i));
        std::vector<bool> state_angle_cont_mask(agent_dofs.at(i), false);
        action_space->setStateAnglesContinuousMask(state_angle_cont_mask);
        action_spaces.push_back(action_space);
    }
    std::cout << BOLDGREEN << "Action spaces created." << RESET << std::endl;

    // Create the heuristics. Each agent has its own list of heuristics, normally only one in each.
    std::vector<std::vector<BaseHeuristic *>> heuristics;
    for (int i = 0; i < num_agents; ++i) {
        std::vector<BaseHeuristic *> agent_heuristics;
        for (const auto &heuristic_name : heuristics_names.at(i)) {
            std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
            auto heuristic_factory = registered_timed_heuristic_factories.find(heuristic_name);
            if (heuristic_factory == registered_timed_heuristic_factories.end()) {
                ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
                throw std::runtime_error("Could not find heuristic factory");
            }
            agent_heuristics.push_back(heuristic_factory->second(articulation_names.at(i), scene_interfaces.at(i)).release());
            std::cout << BOLDGREEN << "OK. xECBS heuristic created for agent " << i << ": " << heuristic_name << RESET << std::endl;
        }
        heuristics.push_back(agent_heuristics);
    }

    // Create the planner.
    EAECBSParams params;
    // Setting params_ecbs.weight_low_level_heuristic;
    std::string weight_low_level_heuristic_str = "55";
    getValueFromVariantMap(planner_context, "w_low_level", weight_low_level_heuristic_str);
    params.weight_low_level_heuristic = std::stod(weight_low_level_heuristic_str);
    std::string weight_focal_high_level_str = "1.3";
    getValueFromVariantMap(planner_context, "w_focal_high_level", weight_focal_high_level_str);
    params.high_level_focal_suboptimality = std::stod(weight_focal_high_level_str);
    std::string weight_focal_low_level_str = "1.3";
    getValueFromVariantMap(planner_context, "w_focal_low_level", weight_focal_low_level_str);
    params.low_level_focal_suboptimality = std::stod(weight_focal_low_level_str);
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params.time_limit_ = std::stod(time_limit);
    params.is_root_trick = true;
    params.experience_reuse_type = ExperienceReuseType::PREVIOUS_SOLUTION;

    params.low_level_heuristic_ptrs.clear();
    for (int i = 0; i < num_agents; ++i) {
        for (auto heuristic: heuristics.at(i)) {
            params.low_level_heuristic_ptrs.push_back(heuristic);
        }
    }

    // Set the conflict and constraints types.
//    params.conflict_type_to_constraint_types = {
//            {ims::ConflictType::VERTEX_POINT3D, {ims::ConstraintType::VERTEX_POINT3D}},
//            {ims::ConflictType::EDGE_POINT3D,   {ims::ConstraintType::EDGE_POINT3D}}
//    };

    planner_cache = std::make_shared<MultiAgentPlanningConfiguration>();
    std::shared_ptr<EAECBS> planner = std::make_shared<EAECBS>(params);
    planner_cache->planner = planner;
    planner_cache->agent_heuristics = heuristics;
    planner_cache->action_spaces = action_spaces;
    planner_cache->planner_name = planner_name;
    planner_cache->agent_heuristics_names = heuristics_names;
    planner_cache->agent_mprim_paths = mprim_paths;
    planner_cache->planner_context = planner_context;
    planner_cache->agent_group_names = articulation_names;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "xECBS planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}


/***
 * @brief This method creates a new PP planner from scratch or reuses one from cache.
 * @param articulation_names The names of the articulation groups being planned for.
 * @param planner_context Planner-specific parameters. E.g., heuristic names, weights, etc.
 * @param scene_interfaces The scene interfaces for each of the agents.
 * @param planner_cache The cache to be used for the planner. If null, a new cache will be created.
 * @return The PP planner.
 */
inline std::shared_ptr<Planner> makeOrUpdatePPPlanner(
            const std::vector<std::string> &articulation_names,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::vector<std::shared_ptr<MoveitInterface>> &scene_interfaces,
            std::shared_ptr<MultiAgentPlanningConfiguration> &planner_cache) {
    std::cout << BOLDGREEN << "Creating PP." << RESET << std::endl;
    // Get all the variables that are needed for verifying the cache (or building a new one).
    std::string planner_name = "PP";
    int num_agents = articulation_names.size();
    // Get the heuristic.
    std::string heuristic_name = "bfs";  // Default heuristic.
    getValueFromVariantMap(planner_context, "heuristic", heuristic_name);
    std::vector<std::vector<std::string>> heuristics_names(num_agents, {heuristic_name});
    // Get the mprim path.
    std::string mprim_path = "";
    getValueFromVariantMap(planner_context, "mprim_path", mprim_path);
    std::vector<std::string> mprim_paths(articulation_names.size(), mprim_path);

    // Check if cache can be reused.
    bool is_cache_reuse = isMultiAgentCacheValid(articulation_names,
                                                  planner_name,
                                                  heuristics_names,
                                                  planner_context,
                                                  scene_interfaces,
                                                  mprim_paths,
                                                  planner_cache);
    if (is_cache_reuse) {
        std::cout << BOLDGREEN << "Reusing cache for PP planner." << RESET << std::endl;
        // If we CAN reuse the cache, simply reset the action spaces and planners.
        planner_cache->planner->resetPlanningData();
        for (auto action_space: planner_cache->action_spaces) {
            action_space->resetPlanningData();
        }
        return planner_cache->planner;
    }
    std::cout << CYAN << "Not reusing cache for PP planner." << RESET << std::endl;
    std::chrono::steady_clock::time_point planner_make_begin = std::chrono::steady_clock::now();

    // DoF of each of the agents.
    std::vector<int> agent_dofs;
    for (int i = 0; i < num_agents; ++i) {
        std::string articulation_name = articulation_names.at(i);
        auto planning_scene_wrapper = scene_interfaces.at(i)->getPlanningSceneMoveit();
        auto planning_scene = std::static_pointer_cast<MoveItPlanningSceneWrapper>(planning_scene_wrapper)->getMoveItPlanningSceneShared();
        int dof = static_cast<int>(planning_scene->getRobotModel()->getJointModelGroup(
                articulation_name)->getVariableCount());
        agent_dofs.push_back(dof);
    }
    std::cout << BOLDGREEN << "Agent DoFs: " << agent_dofs << RESET << std::endl;
    // Create the scene interfaces. This includes deriving the mprim paths.
    // Check if we got an mprim path.
    if (mprim_path.empty()) {
        std::cout << RED << "No mprim path specified for PP planner." << RESET << std::endl;
        // Get the default path.
        for (int i = 0; i < num_agents; ++i) {
            mprim_paths.at(i) = getDefaultMprimPath(agent_dofs.at(i), true);
            std::cout << BOLDGREEN << "Getting default mprim path for " << agent_dofs.at(i) << "dof manipulator: " << mprim_paths.at(i) << RESET << std::endl;
        }
    }


    // Create the action types.
    std::vector<std::shared_ptr<MrampManipulationActionType>> action_types;// = std::make_shared<ManipulationType>(mprim_path);
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionType> action_type = std::make_shared<MrampManipulationActionType>(mprim_paths.at(i));
        // Get resolution from parameters.
        std::string resolution;
        if (!getValueFromVariantMap(planner_context, "resolution", resolution)) {
            throw std::runtime_error("No resolution specified for PP planner.");
        }
        std::vector<double> discretization(agent_dofs.at(i), std::stod(resolution));
        ims::deg2rad(discretization);
        discretization.push_back(1);  // Time is resolution 1.
        std::cout << BOLDGREEN << "Discretization for agent " << i << ": " << discretization << RESET << std::endl;
        action_type->Discretization(discretization);
        action_types.push_back(action_type);
    }

    // Create the snap heuristics.
    std::vector<std::shared_ptr<distance_field::PropagationDistanceField>> dfs;
    std::vector<ims::BFSRemoveTimeHeuristic*> snap_heuristics;
    for (int i = 0; i < num_agents; ++i) {
        auto df_wrapper = scene_interfaces.at(i)->getDistanceFieldMoveItInterface();
        auto df = std::static_pointer_cast<MoveItDistanceFieldWrapper>(df_wrapper)->getMoveItDistanceFieldShared();
        dfs.push_back(df);
        ims::BFSRemoveTimeHeuristic* snap_heuristic = new ims::BFSRemoveTimeHeuristic(df, articulation_names.at(i));
        snap_heuristics.push_back(snap_heuristic);
    }
    // Create the action spaces.
    std::cout << GREEN << "Creating action spaces..." << RESET << std::endl;
    std::vector<std::shared_ptr<ActionSpace>> action_spaces;
    for (int i = 0; i < num_agents; ++i) {
        std::shared_ptr<MrampManipulationActionSpace> action_space =
                std::make_shared<MrampManipulationActionSpace>(std::static_pointer_cast<MoveitInterfaceBase>(scene_interfaces.at(i)),
                                                          *action_types.at(i),
                                                          snap_heuristics.at(i));
        std::vector<bool> state_angle_cont_mask(agent_dofs.at(i), false);
        action_space->setStateAnglesContinuousMask(state_angle_cont_mask);
        action_spaces.push_back(action_space);
    }
    std::cout << BOLDGREEN << "Action spaces created." << RESET << std::endl;

    // Create the heuristics. Each agent has its own list of heuristics, normally only one in each.
    std::vector<std::vector<BaseHeuristic *>> heuristics;
    for (int i = 0; i < num_agents; ++i) {
        std::vector<BaseHeuristic *> agent_heuristics;
        for (const auto &heuristic_name : heuristics_names.at(i)) {
            std::cout << BOLDGREEN << "Creating heuristic: " << heuristic_name << "..." << RESET << std::endl;
            auto heuristic_factory = registered_timed_heuristic_factories.find(heuristic_name);
            if (heuristic_factory == registered_timed_heuristic_factories.end()) {
                ROS_ERROR_STREAM("Could not find heuristic factory for " << heuristic_name);
                throw std::runtime_error("Could not find heuristic factory");
            }
            agent_heuristics.push_back(heuristic_factory->second(articulation_names.at(i), scene_interfaces.at(i)).release());
            std::cout << BOLDGREEN << "OK. PP heuristic created for agent " << i << ": " << heuristic_name << RESET << std::endl;
        }
        heuristics.push_back(agent_heuristics);
    }

    // Create the planner.
    PrioritizedPlanningParams params;
    // Setting params_ecbs.weight_low_level_heuristic;
    std::string weight_low_level_heuristic_str = "55";
    getValueFromVariantMap(planner_context, "w_low_level", weight_low_level_heuristic_str);
    params.weight_low_level_heuristic = std::stod(weight_low_level_heuristic_str);
    std::string time_limit = "10";
    getValueFromVariantMap(planner_context, "allowed_planning_time", time_limit);
    params.time_limit_ = std::stod(time_limit);

    params.low_level_heuristic_ptrs.clear();
    for (int i = 0; i < num_agents; ++i) {
        for (auto heuristic: heuristics.at(i)) {
            params.low_level_heuristic_ptrs.push_back(heuristic);
        }
    }
    params.agent_priority_ordering = {};  // Leave empty for random.

    planner_cache = std::make_shared<MultiAgentPlanningConfiguration>();
    std::shared_ptr<PrioritizedPlanning> planner = std::make_shared<PrioritizedPlanning>(params);
    planner_cache->planner = planner;
    planner_cache->agent_heuristics = heuristics;
    planner_cache->action_spaces = action_spaces;
    planner_cache->planner_name = planner_name;
    planner_cache->agent_heuristics_names = heuristics_names;
    planner_cache->agent_mprim_paths = mprim_paths;
    planner_cache->planner_context = planner_context;
    planner_cache->agent_group_names = articulation_names;

    std::chrono::steady_clock::time_point planner_make_end = std::chrono::steady_clock::now();
    std::cout << BOLDGREEN << "PP planner created from scratch in " << std::chrono::duration_cast<std::chrono::milliseconds>(
            planner_make_end - planner_make_begin).count() << "ms." << RESET << std::endl;

    return planner;
}


using MultiAgentPlannerFactory = std::function<
    std::shared_ptr<ims::Planner>(
            const std::vector<std::string> &articulation_names,
            const std::map<std::string, PlannerContextValueType> &planner_context,
            const std::vector<std::shared_ptr<MoveitInterface>> &scene_interfaces,
            std::shared_ptr<MultiAgentPlanningConfiguration> &planner_cache)
    >;

static std::map<std::string, PlannerFactory> registered_single_agent_planner_factories =
    {
        {"Astar", makeOrUpdateAStarPlanner},
        {"wAstar", makeOrUpdateWAStarPlanner},
        {"ARAstar", makeOrUpdateARAStarPlanner},
        {"MHAstar", makeOrUpdateMHAStarPlanner},
        {"wPASE", makeOrUpdateWPASEPlanner}
    };
static std::map<std::string, MultiAgentPlannerFactory> registered_multi_agent_planner_factories =
    {
        {"ECBS", makeOrUpdateECBSPlanner},
        {"xECBS", makeOrUpdateXECBSPlanner},
        {"PP", makeOrUpdatePPPlanner}
    };

/***
 * @brief A class that emulates a portion of the PlanningWorld class. This class handles the creation of the
 *       different planners and caching of the planning configurations for maximal reuse.
 */
class PlanningWorld {
public:
    // ====================
    // Public variables.
    // ====================
    // Cache for single-robot planning.
    std::shared_ptr <SingleAgentPlanningConfiguration> single_agent_cache_;
    // Cache for multi-robot planning.
    std::shared_ptr <MultiAgentPlanningConfiguration> multi_agent_cache_;

    // ====================
    // Public Methods.
    // ====================
    PlanningWorld();

    /***
     * @brief A method for constructing our planning scene from moveit. This will have better logic later.
     * @param planning_scene The planning scene to be used for planning.
     */
    void setPlanningSceneMoveIt(const planning_scene::PlanningSceneConstPtr &planning_scene);

    /***
     * @brief Create a new planner (+supporting objects like action spaces and heuristics)
     * or update an existing planner with the given configuration.
     * @param articulation_names The names of the move groups to be planned for. Override with just one.
     * @param planner_name The name of the planner to be used.
     * @param heuristic_names The names of the heuristics to be used in the search. A list per agent. Override with just one.
     * @param path_mprim The path to the motion primitives file per robot. Override with just one.
     */
    void makePlanner(const std::vector<std::string> &articulation_names,
                     const std::map<std::string, PlannerContextValueType> &planner_context = {}
    );

    /***
     * @brief Initialize the planner with the given start and goal states. This method creates or updates the internal
     * record of the planner with the given start and goal states.
     * @param start_states The start states of the planning problem. Override with just one.
     * @param goal_states The goal states of the planning problem. Override with just one.
     */
    std::vector<PathType> plan(const std::vector<StateType> &start_states, const std::vector<StateType> &goal_states);

    PathType plan(const StateType &start_state, const StateType &goal_state);

private:
    // ====================
    // Private variables.
    // ====================
    // The scene interface. This will need to change to our own interface. This will be populated in the PlanningWorld
    // by getting information from whichever simulator exists.
    planning_scene::PlanningSceneConstPtr planning_scene_moveit_;
    // ====================
    // Private Methods.
    // ====================
};
} // namespace ims

#endif
