#include <pnkx_local_planner/pnkx_local_planner.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 <angles/angles.h>

#include <tf3/convert.h>
#include <tf3/utils.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>

#include <mkt_msgs/Trajectory2D.h>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>

pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
  : initialized_(false),
    traj_generator_(nullptr),
    nav_algorithm_(nullptr),
    rotate_algorithm_(nullptr),
    goal_checker_(nullptr),
    tf_(nullptr),
    costmap_(nullptr),
    costmap_robot_(nullptr),
    info_(),
    update_costmap_before_planning_(false),
    ret_angle_(false),
    ret_nav_(false),
    yaw_goal_tolerance_(0.0),
    xy_goal_tolerance_(0.0),
    lock_(false)
{
}

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

  if (nav_algorithm_)
    nav_algorithm_.reset();

  if (rotate_algorithm_)
    rotate_algorithm_.reset();

  if (goal_checker_)
    goal_checker_.reset();
}

void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
  if (!initialized_)
  {
    tf_ = tf;
    name_ = name;
    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();

    if (!costmap_robot_->isCurrent())
      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");

    parent_ = parent;
    planner_nh_ = robot::NodeHandle(parent_, name);
    robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
    this->getParams(planner_nh_);

    std::string traj_generator_name;
    planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("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);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
    }
    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_nav_name; 
    planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
      robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
      nav_algorithm_ = nav_algorithm_loader_();
      if (!nav_algorithm_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
      }
      nav_algorithm_->initialize(parent_, algorithm_nav_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_nav_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create algorithm");
    }

    std::string algorithm_rotate_name;
    planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
    try
    {
      robot::PluginLoaderHelper loader;
      std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
        path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
      rotate_algorithm_ = rotate_algorithm_loader_();
      if (!rotate_algorithm_)
      {
        robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
        throw robot_nav_core2::LocalPlannerException("Failed to create rotate algorithm");
      }
      rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate 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 rotate algorithm");
    }

    std::string goal_checker_name;
    planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
    robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
    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");
      }
      goal_checker_->initialize(parent_);
      robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
    }
    catch (const std::exception &ex)
    {
      robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
      throw robot_nav_core2::LocalPlannerException("Failed to create goal checker");
    }

    this->initializeOthers();
    robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str());
    initialized_ = true;
  }
}

void pnkx_local_planner::PNKXLocalPlanner::initializeOthers()
{
}

void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh)
{
  // This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
  nh.param("update_costmap_before_planning", update_costmap_before_planning_, true);
  if (!nh.param<double>("xy_goal_tolerance", xy_goal_tolerance_, 0.1))
  {
    xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1);
  }
  if (!nh.param<double>("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1))
  {
    yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1);
  }
}

void pnkx_local_planner::PNKXLocalPlanner::reset()
{
  this->unlock();
  if(traj_generator_) traj_generator_->reset();
  if(goal_checker_) goal_checker_->reset();
  if(nav_algorithm_) nav_algorithm_->reset();
  if(rotate_algorithm_) rotate_algorithm_->reset();
  ret_nav_ = ret_angle_ = false;
}

void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose)
{
  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  robot::log_error("[PNKXLocalPlanner] Receive new goal(%f, %f)!", goal_pose.pose.x, goal_pose.pose.y);
  reset();
  goal_pose_ = goal_pose;
}

void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path)
{
  robot::log_error("[PNKXLocalPlanner] size path: %d", (int)path.poses.size());
  // boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  costmap_robot_->resetLayers();
  global_plan_ = path;
  if((unsigned int)global_plan_.poses.size() > 2)
    global_plan_.poses.erase(global_plan_.poses.begin());
  for (unsigned int i = 0; i < (unsigned int)global_plan_.poses.size(); i++)
  {
    global_plan_.poses[i].header.seq = i;
  }
}

void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path)
{
  if (local_plan_.poses.empty())
  {
    return;
  }
  path = local_plan_;

}

void pnkx_local_planner::PNKXLocalPlanner::getGlobalPlan(robot_nav_2d_msgs::Path2D &path)
{
  if (global_plan_.poses.empty())
  {
    return;
  }
  path = global_plan_;
}

void pnkx_local_planner::PNKXLocalPlanner::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
  robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
                             local_goal_pose = this->transformPoseToLocal(goal_pose_);

  if (!pnkx_local_planner::transformGlobalPlan(
    tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
  {
    robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
    throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
  }

  double x_direction, y_direction, theta_direction;
  if (!ret_nav_)
  {
    if (nav_algorithm_ && !nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
    {
      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
    }
  }
  else
  {
    if (rotate_algorithm_ && !rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
    {
      robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
      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;
  if(traj_generator_) traj_generator_->setDirect(xytheta_direct);
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::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)
  {
    robot::log_warning_at(__FILE__, __LINE__, "PNKXLocalPlanner computing velocity commands has been broken: %s", e.what());
    throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken");
    return cmd_vel;
  }
}

robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::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_ && ret_angle_)
  {
    return cmd_vel;
    local_plan_.poses.clear();
  }
  else if (!ret_nav_)
  {
    if(nav_algorithm_) 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);
  }
  else
  {
    if(rotate_algorithm_) traj = rotate_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::PNKXLocalPlanner::setTwistLinear(robot_geometry_msgs::Vector3 linear)
{
  try
  {
    if (linear.x == 0.0 && linear.y == 0.0 && linear.z == 0.0)
    {
      if (nav_algorithm_)
        nav_algorithm_->stop();
      if (rotate_algorithm_)
        rotate_algorithm_->stop();
    }
    else
    {
      if (nav_algorithm_)
        nav_algorithm_->resume();
      if (rotate_algorithm_)
        rotate_algorithm_->resume();
    }

    if (traj_generator_)
      return traj_generator_->setTwistLinear(linear);
    else
      throw robot_nav_core2::LocalPlannerException("Failed to set linear");
  }
  catch (const std::exception &e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
    return false;
  }
}

robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
{
  try
  {
    if (traj_generator_)
      return traj_generator_->getTwistLinear(direct);
    else
      throw robot_nav_core2::LocalPlannerException("Failed to get linear");
  }
  catch (const std::exception &e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
  }
}

bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(robot_geometry_msgs::Vector3 angular)
{
  try
  {
    if (traj_generator_)
      return traj_generator_->setTwistAngular(angular);
    else
      throw robot_nav_core2::LocalPlannerException("Failed to set angular");
  }
  catch (const std::exception &e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
    return false;
  }
}

robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
{
  try
  {
    if (traj_generator_)
      return traj_generator_->getTwistAngular(direct);
    else
      throw robot_nav_core2::LocalPlannerException("Failed to get angular");
  }
  catch (const std::exception &e)
  {
    throw robot_nav_core2::LocalPlannerException(e.what());
  }
}

bool pnkx_local_planner::PNKXLocalPlanner::islock()
{
  return lock_;
}

void pnkx_local_planner::PNKXLocalPlanner::lock()
{
  lock_ = true;
}

void pnkx_local_planner::PNKXLocalPlanner::unlock()
{
  lock_ = false;
}

bool pnkx_local_planner::PNKXLocalPlanner::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::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
  robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
  if (!ret_nav_)
  {
    ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);
  }
  else
  {
    double delta_orient = fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
    ret_angle_ = (bool)(delta_orient <= yaw_goal_tolerance_);
  }

  if (ret_nav_ && ret_angle_)
  {
    robot::log_info_throttle(1.0, "Goal reached!");
  }
  return ret_nav_ && ret_angle_;
}

inline double getSquareDistance(const robot_nav_2d_msgs::Pose2DStamped &pose_a, const robot_nav_2d_msgs::Pose2DStamped &pose_b)
{
  double x_diff = pose_a.pose.x - pose_b.pose.x;
  double y_diff = pose_a.pose.y - pose_b.pose.y;
  return x_diff * x_diff + y_diff * y_diff;
}

robot_nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transformPoseToLocal(const robot_nav_2d_msgs::Pose2DStamped &pose)
{
  robot_nav_2d_msgs::Pose2DStamped local_pose;
  robot_nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), pose, local_pose);
  return local_pose;
}

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

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