#include "pnkx_local_planner/pnkx_rotate_local_planner.h"
#include "pnkx_local_planner/transforms.h"

#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <mkt_msgs/Trajectory2D.h>

#include <angles/angles.h>
#include <robot_nav_msgs/Path.h>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>

pnkx_local_planner::PNKXRotateLocalPlanner::PNKXRotateLocalPlanner()
  : PNKXLocalPlanner()
{
}

pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner()
{
  if (traj_generator_)
    traj_generator_.reset();

  if (nav_algorithm_)
    nav_algorithm_.reset();

  if (goal_checker_)
    goal_checker_.reset();
}

void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
{
  if (!initialized_)
  {
    robot::log_info_at(__FILE__, __LINE__, "Using parent name \"%s\"", name.c_str());
    tf_ = tf;
    costmap_robot_ = costmap_robot;
    costmap_ = costmap_robot_->getCostmap();

    info_.width = costmap_->getSizeInCellsX();
    info_.height = costmap_->getSizeInCellsY();
    info_.resolution = costmap_->getResolution();
    info_.frame_id = costmap_robot_->getGlobalFrameID();
    info_.origin_x = costmap_->getOriginX();
    info_.origin_y = costmap_->getOriginY();

    parent_ = parent;
    planner_nh_ = robot::NodeHandle(parent_, name);
    
    this->getParams(planner_nh_);

    std::string traj_generator_name;
    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
    robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(traj_generator_name);
      traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
        path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
      traj_generator_ = traj_gen_loader_();
      if (!traj_generator_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
      }
      robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
      traj_generator_->initialize(nh_traj_gen);
    }
    catch (const std::exception& ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s traj_generator, are you sure it is properly registered and that the containing library is built? Exception: %s", traj_generator_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create traj_generator");
    }

    std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_rotate_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
      nav_algorithm_ = nav_algorithm_loader_();
      if (!nav_algorithm_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
      }
      robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
      nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
    }
    catch (const std::exception& ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
    }

    std::string goal_checker_name;
    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(goal_checker_name);
      goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
        path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
      goal_checker_ = goal_checker_loader_();
      if (!goal_checker_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
      }
      robot::NodeHandle nh_goal_checker = robot::NodeHandle(parent_, goal_checker_name);
      goal_checker_->initialize(nh_goal_checker);
    }
    catch (const std::exception& ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s Goal checker , are you sure it is properly registered and that the containing library is built? Exception: %s", goal_checker_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
    }
    
    robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
    initialized_ = true;
  }
}

void pnkx_local_planner::PNKXRotateLocalPlanner::reset()
{
  robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
  this->unlock();
  if (traj_generator_)
    traj_generator_->reset();
  if (goal_checker_)
    goal_checker_->reset();
  if (nav_algorithm_)
    nav_algorithm_->reset();
  ret_nav_ = ret_angle_ = false;
}

void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
{
  this->getParams(planner_nh_);
  if (update_costmap_before_planning_)
  {
    info_.width = costmap_->getSizeInCellsX();
    info_.height = costmap_->getSizeInCellsY();
    info_.resolution = costmap_->getResolution();
    info_.frame_id = costmap_robot_->getGlobalFrameID();
    info_.origin_x = costmap_->getOriginX();
    info_.origin_y = costmap_->getOriginY();

    if (!costmap_robot_->isCurrent())
      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
  }
  // Update time stamp of goal pose
  // goal_pose_.header.stamp = pose.header.stamp;
  robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
    local_goal_pose = this->transformPoseToLocal(goal_pose_);
  try
  {
    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
      throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
  }
  catch(const robot_nav_core2::LocalPlannerException& e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
  }
  
  double x_direction, y_direction, theta_direction;
  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
  {
    throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
  }

  int xytheta_direct[3];
  xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
  xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
  xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
  traj_generator_->setDirect(xytheta_direct);
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
{
  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  robot_nav_2d_msgs::Twist2DStamped cmd_vel;
  try
  {
    if (global_plan_.poses.empty())
      return cmd_vel;
    this->prepare(pose, velocity);
    cmd_vel = this->ScoreAlgorithm(pose, velocity);
    return cmd_vel;
  }
  catch (const robot_nav_core2::PlannerException &e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
  }
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
{
  robot_nav_2d_msgs::Twist2D twist;
  mkt_msgs::Trajectory2D traj;
  robot_nav_2d_msgs::Twist2DStamped cmd_vel;
  cmd_vel.header.stamp = robot::Time::now();
  if (ret_nav_)
  {
    local_plan_.poses.clear();
    return cmd_vel;
  }

  traj = nav_algorithm_->calculator(pose, velocity);
  local_plan_.header.stamp = robot::Time::now();
  robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
  local_plan_ = robot_nav_2d_utils::pathToPath(path);
  cmd_vel.velocity = traj.velocity;
  return cmd_vel;
}

bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
{
  if (goal_pose_.header.frame_id == "")
  {
    robot::log_warning_at(__FILE__, __LINE__, "Cannot check if the goal is reached without the goal being set!");
    return false;
  }
  // Update time stamp of goal pose
  // goal_pose_.header.stamp = pose.header.stamp;
  robot_nav_2d_msgs::Path2D empty;
  ret_nav_ = goal_checker_->isGoalReached(
    this->transformPoseToLocal(pose), this->transformPoseToLocal(goal_pose_), empty, velocity);

  if (ret_nav_)
    robot::log_info_at(__FILE__, __LINE__, "Goal reached!");
  return ret_nav_;
}

robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
{
  return std::make_shared<pnkx_local_planner::PNKXRotateLocalPlanner>();
}

//  register this planner as a LocalPlanner plugin
BOOST_DLL_ALIAS(pnkx_local_planner::PNKXRotateLocalPlanner::create, PNKXRotateLocalPlanner)