/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Locus Robotics
 *  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 copyright holder 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 HOLDER 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.
 */

#include <robot_nav_core_adapter/global_planner_adapter.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <memory>
#include <string>
#include <vector>

namespace robot_nav_core_adapter
{

  GlobalPlannerAdapter::GlobalPlannerAdapter() 
  //  : planner_loader_("robot_nav_core2", "robot_nav_core2::BaseGlobalPlanner")
  {
  }

  /**
  * @brief Load the robot_nav_core2 global planner and initialize it
  */
  void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name,
                                        TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
  {
    costmap_ = costmap;
    std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<CostmapAdapter>(costmap);

    if (!ptr)
    {
      
      std::cerr <<  "GlobalPlannerAdapter can only be used with the CostmapAdapter, not other Costmaps!" << std::endl;
      exit(EXIT_FAILURE);
    }
    costmap_robot_ = ptr->getCostmap2DROBOT();

    // YAML::Node planner_nh(parent, name);
    std::string planner_name;
    // planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner"));
    // ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
    std::string path_file_so;
    planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
      path_file_so, planner_name, boost::dll::load_mode::append_decorations);
    planner_ = planner_loader_();
    planner_->initialize(planner_name, costmap_robot_);
  }

  robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
                                                      const robot_nav_2d_msgs::Pose2DStamped& goal)
  {
    robot_geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
                              goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
    std::vector<robot_geometry_msgs::PoseStamped> plan;
    bool ret = planner_->makePlan(start3d, goal3d, plan);
    if (!ret)
    {
      throw robot_nav_core2::PlannerException("Generic Global Planner Error");
    }
    return robot_nav_2d_utils::posesToPath2D(plan);
  }

  robot_nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() 
  {
    return std::make_shared<GlobalPlannerAdapter>();
  }
}  // namespace robot_nav_core_adapter

//  register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(robot_nav_core_adapter::GlobalPlannerAdapter::create, create_plugin)
