/*
 * 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/console.h>
#include <robot_nav_core_adapter/local_planner_adapter.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_nav_core_adapter/shared_pointers.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <boost/dll/import.hpp>
#include <memory>
#include <string>
#include <vector>

namespace robot_nav_core_adapter
{

  LocalPlannerAdapter::LocalPlannerAdapter() 
  : initialized_(false), 
    tf_(nullptr), 
    costmap_robot_(nullptr), 
    has_active_goal_(false)
  {
  }

  LocalPlannerAdapter::~LocalPlannerAdapter()
  {
    robot::log_info("=== LocalPlannerAdapter destructor called ===");

    if (planner_)
    {
      planner_.reset();
      robot::log_info("Planner reset complete");
    }
    else
    {
     robot::log_warning("Planner already null");
    }

    robot::log_info("=== LocalPlannerAdapter destructor finished ===");
  }

  /**
   * @brief Load the robot_nav_core2 local planner and initialize it
   */
  void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
  {
    if (!initialized_)
    {
      tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
      costmap_robot_ = costmap_robot;
      costmap_adapter_ = std::make_shared<CostmapAdapter>();
      costmap_adapter_->initialize(costmap_robot);

      private_nh_ = robot::NodeHandle("~");
      adapter_nh_ = robot::NodeHandle(private_nh_, name);
    
      std::string planner_name;
      if (adapter_nh_.hasParam("planner_name"))
      {
        adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
      }
      else
      {
        planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
        adapter_nh_.setParam("planner_name", planner_name);
      }
      try
      {
        robot::PluginLoaderHelper loader;
        std::string path_file_so = loader.findLibraryPath(planner_name);
        planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
          path_file_so, planner_name, boost::dll::load_mode::append_decorations);
        planner_ = planner_loader_();
        if (!planner_)
        {
          robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
          exit(EXIT_FAILURE);
        }
        last_config_.planner_name = planner_name;
        robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
        planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
        robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
      }
      catch (const boost::system::system_error &ex)
      {
        robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what());
        exit(EXIT_FAILURE);
      }
      catch (const std::exception &ex)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
          exit(EXIT_FAILURE);
      }
      
      initialized_ = true;
      robot::log_info("LocalPlannerAdapter initialized successfully");
    }
    else
    {
      robot::log_info("LocalPlannerAdapter already initialized");
    }
  }

  bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
  {
    boost::recursive_mutex::scoped_lock l(configuration_mutex_);
    if (!initialized_)
    {
      return false;
    }
    if(tf_ == nullptr)
    {
      robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
      throw std::runtime_error("Failed to create the tf");
    }
    else
    {
      robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
    }
    if (planner_name != last_config_.planner_name)
    {
      robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
      auto old_loader = planner_loader_;
      try
      {
        boost::unique_lock<boost::recursive_mutex> lock(configuration_mutex_);
        robot::PluginLoaderHelper loader;
        std::string path_file_so = loader.findLibraryPath(planner_name);
        auto new_loader = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
          path_file_so, planner_name, boost::dll::load_mode::append_decorations);

        auto new_planner = new_loader();
        if (!new_planner)
        {
          robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
          throw std::runtime_error("Failed to load planner " + planner_name);
        }
        new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
        planner_ = new_planner;
        planner_loader_ = new_loader;
        last_config_.planner_name = planner_name;
        robot::log_info("Loaded new planner: %s", planner_name.c_str());

        // Release old planner while old loader is still alive.
        old_planner.reset();
        old_loader = boost::function<robot_nav_core2::LocalPlanner::Ptr()>();
        lock.unlock();
      }
      catch (const std::exception &ex)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: %s", planner_name.c_str(), ex.what());
        planner_ = old_planner;
        planner_loader_ = old_loader;
        return false;
      }
    }
    return true;
  }

  /**
   * @brief Collect the additional information needed by robot_nav_core2 and call the child computeVelocityCommands
   */
  bool LocalPlannerAdapter::computeVelocityCommands(const robot_geometry_msgs::Twist &velocity, robot_geometry_msgs::Twist &cmd_vel)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    if (!has_active_goal_)
    {
      return false;
    }

    // Get the Pose
    robot_nav_2d_msgs::Pose2DStamped pose2d;
    if (!getRobotPose(pose2d))
    {
      return false;
    }

    // Get the Velocity
    robot_nav_2d_msgs::Twist2DStamped cmd_vel_2d;
    try
    {
      cmd_vel_2d = planner_->computeVelocityCommands(pose2d, robot_nav_2d_utils::twist3Dto2D(velocity));
    }
    catch (const robot_nav_core2::PlannerException &e)
    {
      robot::log_error_at(__FILE__, __LINE__, "LocalPlannerAdapter computeVelocityCommands exception: %s", e.what());
      return false;
    }
    cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
    return true;
  }

  bool LocalPlannerAdapter::setTwistLinear(robot_geometry_msgs::Vector3 linear)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        return planner_->setTwistLinear(linear);
      else
        throw std::runtime_error("Failed to set linear");
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
  }

  robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        return planner_->getTwistLinear(direct);
      else
        throw std::runtime_error("Failed to get linear");
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
  }

  bool LocalPlannerAdapter::setTwistAngular(robot_geometry_msgs::Vector3 angular)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        return planner_->setTwistAngular(angular);
      else
        throw std::runtime_error("Failed to set angular");
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
  }

  robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        return planner_->getTwistAngular(direct);
      else
        throw std::runtime_error("Failed to get angular");
    }
    catch (const std::exception &e)
    {
      throw std::exception(e);
    }
  }

  robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
  {
    return odom_->twist.twist;
  }

  bool LocalPlannerAdapter::islock()
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        return planner_->islock();
      else
        throw std::runtime_error("Failed to call funcion islock");
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << std::endl;
      throw std::exception(e);
    }
  }

  void LocalPlannerAdapter::lock()
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        planner_->lock();
      else
        throw std::runtime_error("Failed to call funcion lock");
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << std::endl;
      throw std::exception(e);
    }
  }

  void LocalPlannerAdapter::unlock()
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    try
    {
      if (planner_)
        planner_->unlock();
      else
        throw std::runtime_error("Failed to call funcion unlock");
    }
    catch (const std::exception &e)
    {
      std::cerr << e.what() << std::endl;
      throw std::exception(e);
    }
  }

  /**
   * @brief Collect the additional information needed by robot_nav_core2 and call the child isGoalReached
   */
  bool LocalPlannerAdapter::isGoalReached()
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    // Get the Pose
    robot_nav_2d_msgs::Pose2DStamped pose2d;
    if (!getRobotPose(pose2d))
      return false;

    robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
    bool ret = planner_->isGoalReached(pose2d, velocity);
    if (ret)
    {
      has_active_goal_ = false;
    }
    return ret;
  }

  /**
   * @brief Convert from 2d to 3d and call child setPlan
   */
  bool LocalPlannerAdapter::setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &orig_global_plan)
  {
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
    try
    {
      if (path.poses.size() > 0)
      {
        robot_nav_2d_msgs::Pose2DStamped goal_pose;
        goal_pose = path.poses.back();

        if (!has_active_goal_ || hasGoalChanged(goal_pose))
        {
          last_goal_ = goal_pose;
          has_active_goal_ = true;
          planner_->setGoalPose(goal_pose);
          planner_->setPlan(path);
        }
      }
      return true;
    }
    catch (const robot_nav_core2::PlannerException &e)
    {
      std::cerr << "setPlan Exception: " << e.what() << std::endl;
      return false;
    }
  }

  void LocalPlannerAdapter::getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
  {
    if (!planner_)
    {
      return;
    }
    robot_nav_2d_msgs::Path2D path2d;
    planner_->getPlan(path2d);
    path = robot_nav_2d_utils::pathToPath(path2d).poses;
  }

  void LocalPlannerAdapter::getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
  {
    if (!planner_)
    {
      return;
    }
    robot_nav_2d_msgs::Path2D path2d;
    planner_->getGlobalPlan(path2d);
    path = robot_nav_2d_utils::pathToPath(path2d).poses;
  }

  bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
  {
    if (last_goal_.header.frame_id != new_goal.header.frame_id ||
        last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
    {
      return true;
    }

    return last_goal_.pose.x != new_goal.pose.x || last_goal_.pose.y != new_goal.pose.y ||
           last_goal_.pose.theta != new_goal.pose.theta;
  }

  bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
  {
    robot_geometry_msgs::PoseStamped current_pose;
    if (!costmap_robot_->getRobotPose(current_pose))
    {
      std::cout << "Could not get robot pose" << std::endl;
      return false;
    }
    pose2d = robot_nav_2d_utils::poseStampedToPose2D(current_pose);
    return true;
  }

  robot_nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create()
  {
    return std::make_shared<LocalPlannerAdapter>();
  }
} // namespace robot_nav_core_adapter

//  register this planner as a BaseLocalPlanner plugin
BOOST_DLL_ALIAS(robot_nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter)