/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Author: Kevin Lee
 *********************************************************************/
#include <move_base/move_base.h>
#include <cmath>
#include <stdexcept>
#include <cstdlib>
#include <data_convert/data_convert.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <robot/plugin_loader_helper.h>

#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>

move_base::MoveBase::MoveBase()
: initialized_(false),
  planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
  planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
  runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
  pause_ctr_(false), paused_(false)
{
}

move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
: initialized_(false),
  planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
  planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
  runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
  pause_ctr_(false), paused_(false)
{
  initialize(tf);
}

move_base::MoveBase::~MoveBase()
{
  recovery_behaviors_.clear();

  if (planner_costmap_robot_ != NULL)
  {
    delete planner_costmap_robot_;
    planner_costmap_robot_ = NULL;
  }

  if (controller_costmap_robot_ != NULL)
  {
    delete controller_costmap_robot_;
    controller_costmap_robot_ = NULL;
  }

  if (planner_thread_ != NULL)
  {
    planner_thread_->interrupt();
    planner_thread_->join();
    delete planner_thread_;
    planner_thread_ = NULL;
  }

  if (planner_plan_ != NULL)
  {
    delete planner_plan_;
    planner_plan_ = NULL;
  }

  if (latest_plan_ != NULL)
  {
    delete latest_plan_;
    latest_plan_ = NULL;
  }

  if (controller_plan_ != NULL)
  {
    delete controller_plan_;
    controller_plan_ = NULL;
  }

  planner_.reset();
  tc_.reset();
  initialized_ = false;
  tf_.reset();
}

void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
  printf("[%s:%d]\n ========== Begin: initialize() ==========\n", __FILE__, __LINE__);
  if (!initialized_)
  {
    tf_ = tf;

    // NodeHandle("~") will automatically load YAML files from config directory
    private_nh_ =  robot::NodeHandle("~");
    recovery_trigger_ = PLANNING_R;
  
    // get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh_.getParam("base_global_planner", global_planner, std::string(""));
    robot::log_info("[%s %d] global_planner: %s", __FILE__, __LINE__, global_planner.c_str());
    private_nh_.getParam("base_local_planner", local_planner, std::string(""));
    robot::log_info("[%s %d] local_planner: %s", __FILE__, __LINE__, local_planner.c_str());

    // Handle nested YAML nodes for costmap config
    std::string robot_base_frame;
    private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
    std::string global_frame;
    private_nh_.getParam("global_frame", global_frame, std::string("map"));
    robot_base_frame_ = robot_base_frame;
    global_frame_ = global_frame;
    double planner_frequency;
    private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
    planner_frequency_ = planner_frequency;
    double controller_frequency;
    private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
    controller_frequency_ = controller_frequency;
    double planner_patience;
    private_nh_.getParam("planner_patience", planner_patience, 5.0);
    planner_patience_ = planner_patience;
    private_nh_.getParam("controller_patience", controller_patience_, 15.0);
    double max_planning_retries;
    private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
    max_planning_retries_ = max_planning_retries;
    double oscillation_timeout;
    private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
    oscillation_timeout_ = oscillation_timeout;
    double oscillation_distance;
    private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
    oscillation_distance_ = oscillation_distance;

    double original_xy_goal_tolerance;
    private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
    original_xy_goal_tolerance_ = original_xy_goal_tolerance;
    double original_yaw_goal_tolerance;
    private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
    original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;

    // defined local planner name
    private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
    private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
    private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
    private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));

    // parameters of make_plan service
    private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
    private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
    private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
    min_approach_linear_velocity_ *= 1.2;
    // set up plan triple buffer
    planner_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();

    // set up the planner's thread
    planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
    // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    //  From config param
    double inscribed_radius;
    private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
    double circumscribed_radius; 
    private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
    inscribed_radius_ = inscribed_radius;
    circumscribed_radius_ = circumscribed_radius;
    private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
    double conservative_reset_dist;
    private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
    conservative_reset_dist_ = conservative_reset_dist;
    private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
    
    // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    try
    {
      planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
      planner_costmap_robot_->pause();
      robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
      for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
          layer != layered_costmap_->getPlugins()->end();
          ++layer)
      {
        boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
        if (!static_layer)
          continue;
        robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
        occupancy_grid->header.frame_id = "map";
        occupancy_grid->header.stamp = robot::Time::now();
        occupancy_grid->info.resolution = 0.05;
        occupancy_grid->info.width = 100;
        occupancy_grid->info.height = 100;
        occupancy_grid->info.origin.position.x = 0.0;
        occupancy_grid->info.origin.position.y = 0.0;
        occupancy_grid->info.origin.position.z = 0.0;
        occupancy_grid->info.origin.orientation.x = 0.0;
        occupancy_grid->info.origin.orientation.y = 0.0;
        occupancy_grid->info.origin.orientation.z = 0.0;
        occupancy_grid->info.origin.orientation.w = 1.0;
        occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
        for (int i = 0; i < occupancy_grid->data.size(); i++)
        {
          occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
        }
        if (occupancy_grid)
          static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
      }
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the planner_costmap_robot_");
    }
    // initialize the global planner
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath("CustomPlanner");
      planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
          path_file_so, global_planner, boost::dll::load_mode::append_decorations);

      planner_ = planner_loader_();
      if (!planner_)
      {
        robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
        throw std::runtime_error("Failed to load global planner " + global_planner);
      }
      if(planner_->initialize(global_planner, planner_costmap_robot_))
        robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
      else 
        robot::log_error("[%s:%d]\n Global planner initialized failed", __FILE__, __LINE__);
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the " + global_planner + " planner");
    }

    try
    {
      controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
      controller_costmap_robot_->pause();
      robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
      for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
          layer != layered_costmap_->getPlugins()->end();
          ++layer)
      {
        boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
        if (!obstacle_layer)
          continue;
        robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
        laser_scan->header.frame_id = "laser";
        laser_scan->header.stamp = robot::Time::now();
        laser_scan->angle_min = -M_PI;  
        laser_scan->angle_max = M_PI;
        laser_scan->angle_increment = M_PI / 180;
        laser_scan->time_increment = 0.0;
        laser_scan->scan_time = 0.0;
        laser_scan->range_min = 0.0;
        laser_scan->range_max = 10.0;
        laser_scan->ranges.resize(180);
        for (int i = 0; i < 180; i++)
        {
          laser_scan->ranges[i] = 10.0;
        }
        if (laser_scan)
          obstacle_layer->dataCallBack<robot_sensor_msgs::LaserScan>(*laser_scan, "laser");
      }
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the controller_costmap_robot_");
    }
    // create a local planner
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
      controller_loader_ = 
        boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
          path_file_so, local_planner, boost::dll::load_mode::append_decorations);
      tc_ = controller_loader_();
      if (!tc_)
      {
        robot::log_error("[%s:%d]\n ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
        throw std::runtime_error("Failed to load local planner " + local_planner);
      }
      tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
    }
    catch (const std::exception &ex)
    {
      robot::log_error("[%s:%d]\n EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
      throw std::runtime_error("Failed to create the " + local_planner + " planner");
    }

    // Start actively updating costmaps based on sensor data
    planner_costmap_robot_->start();
    controller_costmap_robot_->start();

    try
    {
      old_linear_fwd_ = tc_->getTwistLinear(true);
      old_linear_bwd_ = tc_->getTwistLinear(false);

      old_angular_fwd_ = tc_->getTwistAngular(true);
      old_angular_rev_ = tc_->getTwistAngular(false);
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << '\n';
    }

    // if we shutdown our costmaps when we're deactivated... we'll do that now
    if (shutdown_costmaps_)
    {
      planner_costmap_robot_->stop();
      controller_costmap_robot_->stop();
    }

    // load any user specified recovery behaviors, and if that fails load the defaults
    if (!loadRecoveryBehaviors(private_nh_))
    {
      robot::log_warning("[%s:%d]\n Loading default recovery behaviors", __FILE__, __LINE__);
      loadDefaultRecoveryBehaviors();
    }

    // initially, we'll need to make a plan
    state_ = move_base::PLANNING;

    // we'll start executing recovery behaviors at the beginning of our list
    recovery_index_ = 0;
    nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
      nav_feedback_->is_ready = true;
    }
    else
    { 
      robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
    }
    
    initialized_ = true;
    robot::log_info("========== End: initialize() - SUCCESS ==========");
  }
  else
  {
    robot::log_warning("[%s:%d]\n Already initialized, skipping", __FILE__, __LINE__);
  }
}

void move_base::MoveBase::swapPlanner(std::string base_global_planner)
{
}

void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
{
  if (planner_costmap_robot_)
    planner_costmap_robot_->setUnpaddedRobotFootprint(fprt);

  if (controller_costmap_robot_)
    controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
}

bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
{
  if (setup_)
  {
    swapPlanner(default_config_.base_global_planner);
  }
  else
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (fabs(xy_goal_tolerance) > 0.001)
    this->setXyGoalTolerance(fabs(xy_goal_tolerance));
  else
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));

  try
  {
    tc_->swapPlanner(position_planner_name_);
  }
  catch (const std::exception &e)
  {
    std::cerr << e.what() << "\n";
    throw std::exception(e);
  }

  if (nav_feedback_)
  {
    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
    nav_feedback_->feed_back_str = std::string("Planning");
    nav_feedback_->goal_checked = false;
  }

  if (cancel_ctr_)
    cancel_ctr_ = false;
  lock.unlock();
  return true;
}

bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
                                 const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance, double yaw_goal_tolerance)
{
  return false;
}


bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance, double yaw_goal_tolerance)
{
  std::string maker_sources;
  // private_nh_.param("maker_sources", maker_sources, std::string(""));
  std::stringstream ss(maker_sources);
  std::string source;
  bool has_maker = false;
  while (ss >> source)
  {
    if (maker == source)
    {
      // private_nh_.setParam("maker_name", maker);
      has_maker = true;
    }
  }
  if (!has_maker)
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      std::stringstream fb_ss;
      fb_ss << "The system has not been '" << maker << "'";
      nav_feedback_->feed_back_str = fb_ss.str();
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (setup_)
  {
    swapPlanner(default_config_.base_global_planner);
  }
  else
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (fabs(xy_goal_tolerance) > 0.001)
    this->setXyGoalTolerance(fabs(xy_goal_tolerance));
  else
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  try
  {
    tc_->swapPlanner(docking_planner_name_);
  }
  catch (const std::exception &e)
  {
    std::cerr << e.what() << "\n";
    throw std::exception(e);
  }

  if (nav_feedback_)
  {
    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
    nav_feedback_->feed_back_str = std::string("Planning");
    nav_feedback_->goal_checked = false;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;

  lock.unlock();
  return true;
}

bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
                                 const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance, double yaw_goal_tolerance)
{
  return false;
}

bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
  if (setup_)
  {
    std::string global_planner = "TwoPointsPlanner";
    swapPlanner(global_planner);
  }
  else
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  if (fabs(xy_goal_tolerance) > 0.001)
    this->setXyGoalTolerance(fabs(xy_goal_tolerance));
  else
    this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));

  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  tc_->swapPlanner(go_straight_planner_name_);

  if (nav_feedback_)
  {
    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
    nav_feedback_->feed_back_str = std::string("Planning");
    nav_feedback_->goal_checked = false;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;

  lock.unlock();
  return true;
}

bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
{
  if (setup_)
  {
    std::string global_planner = "TwoPointsPlanner";
    swapPlanner(global_planner);
  }
  else
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }

  this->setXyGoalTolerance(0.25);

  if (fabs(yaw_goal_tolerance) > 0.001)
    this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
  else
    this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));

  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  tc_->swapPlanner(rotate_planner_name_);

  if (nav_feedback_)
  {
    nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
    nav_feedback_->feed_back_str = std::string("Planning");
    nav_feedback_->goal_checked = false;
  }
  if (cancel_ctr_)
    cancel_ctr_ = false;
  robot_geometry_msgs::PoseStamped pose;
  if (!this->getRobotPose(pose))
  {
    if (nav_feedback_)
    {
      nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
      nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
      nav_feedback_->goal_checked = false;
    }
    return false;
  }
  lock.unlock();
  return true;
}

void move_base::MoveBase::pause()
{
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }

  try
  {
    old_linear_fwd_ = tc_->getTwistLinear(true);
    old_linear_bwd_ = tc_->getTwistLinear(false);

    old_angular_fwd_ = tc_->getTwistAngular(true);
    old_angular_rev_ = tc_->getTwistAngular(false);
  }
  catch (const std::exception &e)
  {
    std::cerr << e.what() << '\n';
  }
  pause_ctr_ = true;
  paused_ = false;
}

void move_base::MoveBase::resume()
{
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  if (!tc_)
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  tc_->setTwistLinear(old_linear_fwd_);
  tc_->setTwistLinear(old_linear_bwd_);

  tc_->setTwistAngular(old_angular_fwd_);
  tc_->setTwistAngular(old_angular_rev_);

  pause_ctr_ = false;
  paused_ = false;
}

void move_base::MoveBase::cancel()
{
  if (!controller_costmap_robot_)
  {
    throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
  }
  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
  cancel_ctr_ = true;
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
{
  if (!initialized_)
    return false;
  this->getRobotPose(pose, planner_costmap_robot_);
  return true;
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::Pose2D &pose)
{
  if (!initialized_)
    return false;
  robot_geometry_msgs::PoseStamped global_pose;
  this->getRobotPose(global_pose, planner_costmap_robot_);
  pose.x = global_pose.pose.position.x;
  pose.y = global_pose.pose.position.y;

  tf3::Quaternion quaternion = tf3::Quaternion(
      global_pose.pose.orientation.x,
      global_pose.pose.orientation.y,
      global_pose.pose.orientation.z,
      global_pose.pose.orientation.w);

  pose.theta = tf3::getYaw(quaternion);
  return true;
}

bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &linear)
{
  try
  {
    if (tc_ && !cancel_ctr_)
    {
      if (tc_->islock())
        return true;

      return tc_->setTwistLinear(linear);
    }
    else if (tc_ && cancel_ctr_)
    {
      tc_->unlock();
      return false;
    }
    else
    {
      throw std::runtime_error("Null 'tc_' pointer encountered");
    }
  }
  catch (const std::exception &e)
  {
    return false;
  }
}

bool move_base::MoveBase::setTwistAngular(const robot_geometry_msgs::Vector3 &angular)
{
  if (tc_ && !cancel_ctr_)
  {
    if (tc_->islock())
      return true;
    return tc_->setTwistAngular(angular);
  }
  else if (tc_ && cancel_ctr_)
  {
    return false;
  }
  else
  {
    throw std::runtime_error("Null 'tc_' pointer encountered");
  }
  return false;
}

void move_base::MoveBase::setYawGoalTolerance(double yaw_goal_tolerance)
{
  // double yaw = 0.0;
  // if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0))
  // {
  //   private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance);
  //   std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl;
  // }
  // else
  //   std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
}

void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance)
{
  // double xy = 0.0;
  // if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0))
  // {
  //   private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance);
  //   std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl;
  // }
  // else
  //   std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl;
}

// bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
// {
// }

// bool planService(robot_nav_msgs::GetPlan::Request &req, robot_nav_msgs::GetPlan::Response &resp)
// {

// }

bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
  // make sure to set the plan to be empty initially
  plan.clear();

  // since this gets called on handle activate
  if (!planner_costmap_robot_)
  {
    std::cerr << "Planner costmap ROS is NULL, unable to create global plan" << std::endl;
    return false;
  }

  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));

  // get the starting pose of the robot
  robot_geometry_msgs::PoseStamped global_pose;
  if (!getRobotPose(global_pose, planner_costmap_robot_))
  {
    std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl;
    return false;
  }

  const robot_geometry_msgs::PoseStamped &start = global_pose;

  // if the planner fails or returns a zero length plan, planning failed
  if (!planner_->makePlan(start, goal, plan) || plan.empty())
  {
    std::cout << "Failed to find a  plan to point (" << goal.pose.position.x << ", " << goal.pose.position.y << ")" << std::endl;
    return false;
  }
  lock.unlock();
  return true;
}

bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
{
  // YAML::Node behavior_list = node.getParamValue("recovery_behaviors");
  // if (behavior_list && behavior_list.IsSequence())
  // {
  //   // Validate all behaviors first
  //   for (size_t i = 0; i < behavior_list.size(); ++i)
  //   {
  //     YAML::Node behavior = behavior_list[i].as<YAML::Node>();
  //     if (!behavior.IsMap())
  //     {
  //       robot::log_warning("Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead.");
  //       return false;
  //     }
      
  //     if (!behavior["name"] || !behavior["type"])
  //     {
  //       robot::log_warning("Recovery behaviors must have a name and a type. Using the default recovery behaviors instead.");
  //       return false;
  //     }
      
  //     // check for recovery behaviors with the same name
  //     std::string name_i = behavior["name"].as<std::string>();
  //     for (size_t j = i + 1; j < behavior_list.size(); ++j)
  //     {
  //       YAML::Node behavior_j = behavior_list[j].as<YAML::Node>();
  //       if (behavior_j.IsMap() && behavior_j["name"])
  //       {
  //         std::string name_j = behavior_j["name"].as<std::string>();
  //         if (name_i == name_j)
  //         {
  //           robot::log_warning("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", name_i.c_str());
  //           return false;
  //         }
  //       }
  //     }
  //   }

  //   // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
  //   for (size_t i = 0; i < behavior_list.size(); ++i)
  //   {
  //     try
  //     {
  //       YAML::Node behavior = behavior_list[i].as<YAML::Node>();
  //       // Load recovery behavior using boost::dll::import_alias
  //       // behavior["type"] should be the factory function name (e.g., "create_plugin")
  //       std::string behavior_type = behavior["type"].as<std::string>();
  //       std::string behavior_name = behavior["name"].as<std::string>();
  //       std::string path_file_so;
  //       // Load the factory function from the shared library
  //       auto recovery_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
  //           path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
        
  //       // Create instance using the factory function
  //       std::shared_ptr<nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
        
  //       // shouldn't be possible, but it won't hurt to check
  //       if (behavior_ptr == nullptr)
  //       {
  //         std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl;
  //         return false;
  //       }
  //       // initialize the recovery behavior with its name
  //       behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
  //       recovery_behavior_names_.push_back(behavior_name);
  //       recovery_behaviors_.push_back(behavior_ptr);
  //     }
  //     catch (const std::exception &ex)
  //     {
  //       std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as<std::string>()  
  //                 << ". Error: " << ex.what() << std::endl;
  //       return false;
  //     }
  //   }
  // }
  // else if (behavior_list)
  // {
  //   std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl;
  //   return false;
  // }
  // else
  // {
  //   // if no recovery_behaviors are specified, we'll just load the defaults
  //   return false;
  // }

  // if we've made it here... we've constructed a recovery behavior list successfully
  return true;
}

void move_base::MoveBase::loadDefaultRecoveryBehaviors()
{
  recovery_behaviors_.clear();
  recovery_behavior_names_.clear();
  
  try
  {
    // Note: Parameters should be set via YAML config file, not programmatically
    // The recovery behaviors will read their parameters from the YAML node during initialization

    // first, we'll load a recovery behavior to clear the costmap (conservative reset)
    try
    {
      std::string path_file_so ;
      auto clear_costmap_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
          path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
      std::shared_ptr<nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
      cons_clear->initialize("conservative_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
      recovery_behavior_names_.push_back("conservative_reset");
      recovery_behaviors_.push_back(cons_clear);
    }
    catch (const std::exception &ex)
    {
      std::cerr << "Failed to load clear_costmap_recovery plugin: " << ex.what() << std::endl;
    }

    // next, we'll load a recovery behavior to rotate in place
    std::shared_ptr<nav_core::RecoveryBehavior> rotate;
    if (clearing_rotation_allowed_)
    {
      try
      {
        std::string path_file_so;
        auto rotate_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
            path_file_so, "rotate_recovery", boost::dll::load_mode::append_decorations);
        rotate = std::shared_ptr<nav_core::RecoveryBehavior>(rotate_loader());
        rotate->initialize("rotate_recovery", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
        recovery_behavior_names_.push_back("rotate_recovery");
        recovery_behaviors_.push_back(rotate);
      }
      catch (const std::exception &ex)
      {
        std::cerr << "Failed to load rotate_recovery plugin: " << ex.what() << std::endl;
      }
    }

    // next, we'll load a recovery behavior that will do an aggressive reset of the costmap
    try
    {
      std::string path_file_so;
      auto clear_costmap_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
          path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
      std::shared_ptr<nav_core::RecoveryBehavior> ags_clear(clear_costmap_loader());
      ags_clear->initialize("aggressive_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
      recovery_behavior_names_.push_back("aggressive_reset");
      recovery_behaviors_.push_back(ags_clear);
    }
    catch (const std::exception &ex)
    {
      std::cerr << "Failed to load clear_costmap_recovery plugin for aggressive reset: " << ex.what() << std::endl;
    }

    // we'll rotate in-place one more time
    if (clearing_rotation_allowed_ && rotate)
    {
      recovery_behaviors_.push_back(rotate);
      recovery_behavior_names_.push_back("rotate_recovery");
    }
  }
  catch (const std::exception &ex)
  {
    std::cerr << "Failed to load default recovery behaviors. Error: " << ex.what() << std::endl;
  }

  return;
}

void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
{
  robot_geometry_msgs::PoseStamped global_pose;

  // clear the planner's costmap
  getRobotPose(global_pose, planner_costmap_robot_);

  std::vector<robot_geometry_msgs::Point> clear_poly;
  double x = global_pose.pose.position.x;
  double y = global_pose.pose.position.y;
  robot_geometry_msgs::Point pt;

  pt.x = x - size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x - size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);

  // clear the controller's costmap
  getRobotPose(global_pose, controller_costmap_robot_);

  clear_poly.clear();
  x = global_pose.pose.position.x;
  y = global_pose.pose.position.y;

  pt.x = x - size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y - size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x + size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  pt.x = x - size_x / 2;
  pt.y = y + size_y / 2;
  clear_poly.push_back(pt);

  controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
}

void move_base::MoveBase::publishZeroVelocity()
{
}

void move_base::MoveBase::resetState()
{
  // Disable the planner thread
  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  runPlanner_ = false;
  lock.unlock();

  // Reset statemachine
  state_ = move_base::PLANNING;

  recovery_index_ = 0;
  recovery_trigger_ = PLANNING_R;
  paused_ = false;
  publishZeroVelocity();

  // if we shutdown our costmaps when we're deactivated... we'll do that now
  if (shutdown_costmaps_)
  {
    std::cout << "Stopping costmaps" << std::endl;
    planner_costmap_robot_->stop();
    controller_costmap_robot_->stop();
  }
}

// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);

void move_base::MoveBase::planThread()
{
  // std::cout << "Starting planner thread..." << std::endl;
  // YAML::Node n("~");
  // robot::Timer timer;
  // bool wait_for_wake = false;
  // boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  // while (true)
  // {
  //   // check if we should run the planner (the mutex is locked)
  //   while (wait_for_wake || !runPlanner_)
  //   {
  //     // if we should not be running the planner then suspend this thread
  //     std::cout << "Planner thread is suspending" << std::endl;
  //     planner_cond_.wait(lock);
  //     wait_for_wake = false;
  //   }
  //   robot::Time start_time = robot::Time::now();

  //   // time to plan! get a copy of the goal and unlock the mutex
  //   robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
  //   lock.unlock();
  //   std::cout << "Planning..." << std::endl;
  //   // run planner
  //   planner_plan_->clear();
  //   // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
  //   bool gotPlan =  makePlan(temp_goal, *planner_plan_);
    
  //   if (gotPlan)
  //   {
  //     std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl;
  //     // pointer swap the plans under mutex (the controller will pull from latest_plan_)
  //     std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;

  //     lock.lock();
  //     planner_plan_ = latest_plan_;
  //     latest_plan_ = temp_plan;
  //     last_valid_plan_ = robot::Time::now();
  //     planning_retries_ = 0;
  //     new_global_plan_ = true;

  //     std::cout << "Generated a plan from the base_global_planner" << std::endl;

  //     // make sure we only start the controller if we still haven't reached the goal
  //     if (runPlanner_)
  //     {
  //       state_ = move_base::CONTROLLING;
  //       if (nav_feedback_)
  //         nav_feedback_->navigation_state = move_base_core::State::CONTROLLING;
  //     }
  //     if (planner_frequency_ <= 0)
  //       runPlanner_ = false;
  //     lock.unlock();
  //   }
  //   // if we didn't get a plan and we are in the planning state (the robot isn't moving)
  //   else if (state_ == move_base::PLANNING)
  //   {
  //     std::cout << "No Plan..." << std::endl;
  //     robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);

  //     // check if we've tried to make a plan for over our time limit or our maximum number of retries
  //     // issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
  //     // is negative (the default), it is just ignored and we have the same behavior as ever
  //     lock.lock();
  //     planning_retries_++;
  //     if (runPlanner_ &&
  //         (robot::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)))
  //     {
  //       // we'll move into our obstacle clearing mode
  //       state_ = move_base::CLEARING;
  //       if (nav_feedback_)
  //         nav_feedback_->navigation_state = move_base_core::State::CLEARING;

  //       runPlanner_ = false; // proper solution for issue #523
  //       publishZeroVelocity();
  //       recovery_trigger_ = PLANNING_R;
  //     }

  //     lock.unlock();
  //   }

  //   // take the mutex for the next iteration
  //   lock.lock();

  //   // setup sleep interface if needed
  //   if (planner_frequency_ > 0)
  //   {
  //     robot::Duration sleep_time = (start_time + robot::Duration(1.0 / planner_frequency_)) - robot::Time::now();
  //     if (sleep_time > robot::Duration(0.0))
  //     {
  //       wait_for_wake = true;
  //       timer = robot::Timer(sleep_time, &move_base::MoveBase::wakePlanner, this);
  //     }
  //   }
  // }
}

// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);

bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternion &q)
{
  // first we need to check if the quaternion has nan's or infs
  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
  {
    std::cerr << "Quaternion has nans or infs... discarding as a navigation goal" << std::endl;
    return false;
  }

  tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);

  // next, we need to check if the length of the quaternion is close to zero
  if (tf_q.length2() < 1e-6)
  {
    std::cerr << "Quaternion has length close to zero... discarding as navigation goal" << std::endl;
    return false;
  }

  // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
  tf_q.normalize();

  tf3::Vector3 up(0, 0, 1);

  double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

  if (fabs(dot - 1) > 1e-3)
  {
    std::cerr << "Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical." << std::endl;
    return false;
  }

  return true;
}

bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
{
  
  tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
  robot_geometry_msgs::PoseStamped robot_pose;
  tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
  robot_pose.header.frame_id = robot_base_frame_;
  robot_pose.header.stamp = robot::Time();     // latest available
  robot::Time current_time = robot::Time::now(); // save time for checking tf delay later

  // get robot pose on the given costmap frame
  try
  {
    tf3::TransformStampedMsg transform = tf_->lookupTransform(costmap->getGlobalFrameID(), robot_base_frame_, data_convert::convertTime(current_time));
    tf3::doTransform(robot_pose, global_pose, transform);
  }
  catch (tf3::LookupException &ex)
  {
    std::cerr << "Move Base No Transform available Error looking up robot pose: " << ex.what() << std::endl;
    return false;
  }
  catch (tf3::ConnectivityException &ex)
  {
    std::cerr << "Connectivity Error looking up robot pose: " << ex.what() << std::endl;
    return false;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl;
    return false;
  }

  // check if global_pose time stamp is within costmap transform tolerance
  if (!global_pose.header.stamp.isZero() &&
      current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
  {
    std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". "
                           "Current time: " << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl;
    return false;
  }

  return true;
}

double move_base::MoveBase::distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2)
{
  return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}

robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
{
  std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
  robot_geometry_msgs::PoseStamped goal_pose, global_pose;
  goal_pose = goal_pose_msg;

  goal_pose.header.stamp = robot::Time();     // latest available
  try
  {
    tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp));
    tf3::doTransform(goal_pose, global_pose, transform);
  }
  catch (tf3::LookupException &ex)
  {
    std::cerr << "Move Base No Transform available Error looking up robot pose: " << ex.what() << std::endl;
    return goal_pose_msg;
  }
  catch (tf3::ConnectivityException &ex)
  {
    std::cerr << "Connectivity Error looking up robot pose: " << ex.what() << std::endl;
    return goal_pose_msg;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl;
    return goal_pose_msg;
  }
  return global_pose;
}

bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
{
  if (!twist_) {
    return false;
  }
  twist = *twist_;
  return true;
}

bool move_base::MoveBase::getFeedback(robot::move_base_core::NavFeedback &feedback)
{
  if (!nav_feedback_) {
    return false;
  }
  feedback = *nav_feedback_;
  return true;
}

// void wakePlanner(const robot::TimerEvent &event)
// {
// }

robot::move_base_core::BaseNavigation::Ptr move_base::MoveBase::create()
{
  return std::make_shared<move_base::MoveBase>();
}

BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase)