/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — cài đặt ControllerRunner.
 *
 * Author: DuongTD
 *********************************************************************/
#include <move_base2/runners/controller_runner.h>

#include <cmath>
#include <exception>
#include <sstream>
#include <utility>

#include <boost/dll/import.hpp>
#include <boost/system/system_error.hpp>

#include <robot/plugin_loader_helper.h>
#include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_nav_2d_utils/conversions.h>

namespace move_base2
{
namespace
{

/// [s] Giãn cách log cho cảnh báo phát sinh trong đường nóng — nhịp control loop, không được spam.
constexpr double kHotPathLogThrottle = 5.0;

bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
{
  return std::isfinite(twist.linear.x) && std::isfinite(twist.linear.y) &&
         std::isfinite(twist.angular.z);
}

}  // namespace

ControllerRunner::ControllerRunner() = default;
ControllerRunner::~ControllerRunner() = default;

void ControllerRunner::attachStats(RuntimeStats* stats)
{
  stats_ = stats;
  if (stats_ == nullptr)
  {
    return;
  }
  section_compute_ = stats_->section("controller.compute");
  section_local_plan_ = stats_->section("controller.getLocalPlan");
}

bool ControllerRunner::configure(const robot::NodeHandle& nh,
                                 const std::shared_ptr<tf3::BufferCore>& tf,
                                 robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
                                 const std::string& initial_controller, std::string& error)
{
  if (configured_)
  {
    error = "ControllerRunner::configure() called twice";
    return false;
  }

  if (costmap == nullptr)
  {
    error = "ControllerRunner needs a non-null local costmap";
    return false;
  }

  if (pose == nullptr)
  {
    // Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands/isGoalReached; không có nguồn pose
    // thì không gọi được hàm nào trong hai hàm đó.
    error = "ControllerRunner needs a non-null PosePort";
    return false;
  }

  nh_ = nh;
  tf_ = tf;
  costmap_ = costmap;
  pose_ = pose;
  configured_ = true;

  if (!initial_controller.empty() && !swapPlanner(initial_controller))
  {
    error = "could not load the initial local planner '" + initial_controller + "'";
    configured_ = false;
    return false;
  }

  return true;
}

robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name)
{
  const auto cached = controllers_.find(name);
  if (cached != controllers_.end())
  {
    if (!marker_dirty_)
    {
      return cached->second.instance.get();
    }

    // Marker vừa đổi: instance này có thể đã đọc `maker_name` trong initialize() và không bao giờ
    // đọc lại. Dựng lại từ factory sẵn có (không dlopen lại); instance mới thay chỗ instance cũ
    // CHỈ khi initialize() thành công — thất bại thì giữ nguyên cache và trả lỗi để bên gọi từ
    // chối yêu cầu, không để lại trạng thái nửa vời.
    try
    {
      robot_nav_core2::LocalPlanner::Ptr fresh = cached->second.factory();
      if (!fresh)
      {
        robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
                         "rebuilding for the new marker.\n", name.c_str());
        return nullptr;
      }
      fresh->initialize(nh_, name, tf_, costmap_);
      cached->second.instance = std::move(fresh);
    }
    catch (const std::exception& ex)
    {
      robot::log_error("[move_base2] ControllerRunner: rebuilding '%s' for the new marker failed: "
                       "%s\n",
                       name.c_str(), ex.what());
      return nullptr;
    }
    marker_dirty_ = false;
    return cached->second.instance.get();
  }

  robot::PluginLoaderHelper loader(nh_);
  const std::string library_path = loader.findLibraryPath(name);

  if (library_path.empty())
  {
    robot::log_error("[move_base2] ControllerRunner: no library found for '%s' — check the key "
                     "'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
                     name.c_str(), name.c_str());
    return nullptr;
  }

  Loaded loaded;

  try
  {
    loaded.factory = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
        library_path, name, boost::dll::load_mode::append_decorations);
  }
  catch (const boost::system::system_error& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: could not load symbol '%s' from '%s': %s\n",
                     name.c_str(), library_path.c_str(), ex.what());
    return nullptr;
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: error while loading '%s': %s\n", name.c_str(),
                     ex.what());
    return nullptr;
  }

  try
  {
    loaded.instance = loaded.factory();
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: factory of '%s' threw an exception: %s\n",
                     name.c_str(), ex.what());
    return nullptr;
  }

  if (!loaded.instance)
  {
    robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr.\n",
                     name.c_str());
    return nullptr;
  }

  try
  {
    // Chữ ký gen-2: (parent NodeHandle, tên, TF, costmap). Khác hẳn gen-1 — và vì Boost.DLL không
    // kiểm kiểu, gọi nhầm chữ ký sẽ không lỗi biên dịch mà hỏng vtable lúc chạy.
    loaded.instance->initialize(nh_, name, tf_, costmap_);
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: initialize() of '%s' threw an exception: %s\n",
                     name.c_str(), ex.what());
    return nullptr;
  }

  const auto inserted = controllers_.emplace(name, std::move(loaded));
  // Instance mới vừa initialize() với `maker_name` hiện hành — marker không còn "chưa được đọc".
  marker_dirty_ = false;
  return inserted.first->second.instance.get();
}

bool ControllerRunner::setDockingMarker(const std::string& marker)
{
  if (!configured_)
  {
    robot::log_error("[move_base2] ControllerRunner: setDockingMarker() before configure().\n");
    return false;
  }

  // Validate với danh sách `maker_sources` (chuỗi cách nhau bằng space, maker_sources.yaml) —
  // đúng phép kiểm bản cũ làm ở cửa dockTo (move_base.cpp:1161-1173). Marker lạ phải bị chặn ở
  // đây: để lọt xuống thì getMaker() của docking planner âm thầm không match source nào và robot
  // đứng im không lý do.
  std::string sources;
  nh_.param("maker_sources", sources, std::string(""));
  std::stringstream ss(sources);
  std::string source;
  bool known = false;
  while (ss >> source)
  {
    if (source == marker)
    {
      known = true;
      break;
    }
  }
  if (!known)
  {
    robot::log_error("[move_base2] ControllerRunner: marker '%s' is not listed in maker_sources "
                     "('%s').\n", marker.c_str(), sources.c_str());
    return false;
  }

  std::string current;
  nh_.param("maker_name", current, std::string(""));
  if (current != marker)
  {
    nh_.setParam("maker_name", marker);
    marker_dirty_ = true;
  }
  return true;
}

void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller)
{
  if (controller == nullptr)
  {
    return;
  }

  // Trần vận tốc thuộc về YÊU CẦU, không thuộc về instance planner. Instance mới nạp không biết gì
  // về các trần host đã đặt trước đó — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an
  // toàn cho phép, và không có dấu hiệu nào cả.
  try
  {
    if (has_limit_linear_forward_)
    {
      controller->setTwistLinear(limit_linear_forward_);
    }
    if (has_limit_linear_backward_)
    {
      controller->setTwistLinear(limit_linear_backward_);
    }
    if (has_limit_angular_)
    {
      controller->setTwistAngular(limit_angular_);
    }
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: error while re-applying the velocity limits: "
                     "%s\n", ex.what());
  }
}

bool ControllerRunner::swapPlanner(const std::string& planner_name)
{
  if (!configured_)
  {
    robot::log_error("[move_base2] ControllerRunner: swapPlanner() before configure().\n");
    return false;
  }

  if (planner_name.empty())
  {
    robot::log_error("[move_base2] ControllerRunner: empty controller name.\n");
    return false;
  }

  if (planner_name == active_name_ && active_ != nullptr && !marker_dirty_)
  {
    // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu. Riêng khi marker vừa đổi
    // thì KHÔNG được đi tắt: dock lại cùng planner với marker khác phải rơi xuống acquire() để
    // instance được dựng lại và initialize() đọc `maker_name` mới.
    return true;
  }

  robot_nav_core2::LocalPlanner* controller = acquire(planner_name);
  if (controller == nullptr)
  {
    // Giữ nguyên controller đang chạy: bên gọi từ chối yêu cầu dựa vào giá trị trả về.
    return false;
  }

  active_ = controller;
  active_name_ = planner_name;
  has_active_goal_ = false;  // Instance mới chưa biết goal nào.
  active_plan_.clear();
  applyPendingLimits(active_);

  robot::log_info("[move_base2] ControllerRunner: active local planner is '%s'.\n",
                  planner_name.c_str());
  return true;
}

bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
  if (!configured_ || active_ == nullptr)
  {
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: setPlan() with no controller "
                              "loaded.\n");
    return false;
  }

  if (plan.empty())
  {
    // Plan rỗng lọt xuống sẽ thành front()/back() trên vector rỗng bên trong planner.
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: rejecting an empty plan.\n");
    return false;
  }

  try
  {
    // Gen-2 tách goal khỏi plan: `setGoalPose` phải gọi TRƯỚC `setPlan`, đúng như
    // `LocalPlannerAdapter` làm. Goal là pose CUỐI của plan.
    const robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(plan);
    if (path.poses.empty())
    {
      robot::log_error_throttle(kHotPathLogThrottle,
                                "[move_base2] ControllerRunner: plan converted to Path2D came out "
                                "empty.\n");
      return false;
    }

    // `Path2D::poses` đã là vector<Pose2DStamped>, nên pose cuối dùng thẳng làm goal.
    const robot_nav_2d_msgs::Pose2DStamped goal_pose = path.poses.back();

    active_->setGoalPose(goal_pose);
    active_->setPlan(path);
    has_active_goal_ = true;
    active_plan_ = plan;
    return true;
  }
  catch (const std::exception& ex)
  {
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: '%s' threw an exception in setPlan: "
                              "%s\n", active_name_.c_str(), ex.what());
    return false;
  }
}

bool ControllerRunner::refreshActivePlanner()
{
  if (!configured_ || active_ == nullptr)
  {
    // Chưa có controller active thì không có cache nào cần refresh. Hành vi này giúp footprint có
    // thể được host đặt trước goal đầu tiên mà không biến thành lỗi khởi tạo.
    return true;
  }

  const auto loaded = controllers_.find(active_name_);
  if (loaded == controllers_.end())
  {
    robot::log_error("[move_base2] ControllerRunner: active local planner '%s' is absent from "
                     "the plugin cache.\n", active_name_.c_str());
    return false;
  }

  const bool had_active_goal = has_active_goal_;
  const std::vector<robot_geometry_msgs::PoseStamped> saved_plan = active_plan_;
  if (had_active_goal && saved_plan.empty())
  {
    // Không thay instance cũ nếu không thể khôi phục goal đang chạy. Giữ controller hiện tại vẫn
    // an toàn hơn việc âm thầm biến navigation thành controller không có plan.
    robot::log_error("[move_base2] ControllerRunner: active planner '%s' has a goal but no "
                     "cached plan to restore after a footprint change.\n", active_name_.c_str());
    return false;
  }

  robot_nav_core2::LocalPlanner::Ptr fresh;
  try
  {
    fresh = loaded->second.factory();
    if (!fresh)
    {
      robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
                       "refreshing its footprint cache.\n", active_name_.c_str());
      return false;
    }
    fresh->initialize(nh_, active_name_, tf_, costmap_);
  }
  catch (const std::exception& ex)
  {
    // Chỉ thay cache SAU initialize thành công, nên lỗi này không làm mất controller cũ.
    robot::log_error("[move_base2] ControllerRunner: refreshing '%s' after a footprint change "
                     "failed: %s\n", active_name_.c_str(), ex.what());
    return false;
  }

  loaded->second.instance = std::move(fresh);
  active_ = loaded->second.instance.get();
  has_active_goal_ = false;
  active_plan_.clear();
  applyPendingLimits(active_);

  if (had_active_goal && !setPlan(saved_plan))
  {
    robot::log_error("[move_base2] ControllerRunner: could not restore the active plan after "
                     "refreshing '%s' for a footprint change.\n", active_name_.c_str());
    return false;
  }

  robot::log_info("[move_base2] ControllerRunner: refreshed '%s' after the local costmap "
                  "footprint changed.\n", active_name_.c_str());
  return true;
}

bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
{
  if (pose_ == nullptr)
  {
    return false;
  }
  robot_geometry_msgs::PoseStamped stamped;
  if (!pose_->getRobotPose(stamped))
  {
    return false;
  }
  pose = robot_nav_2d_utils::poseStampedToPose2D(stamped);
  return true;
}

bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
{
  cmd = robot_geometry_msgs::Twist();

  if (!configured_ || active_ == nullptr)
  {
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: computeVelocityCommands() with no "
                              "controller loaded.\n");
    return false;
  }

  if (!has_active_goal_)
  {
    // Chưa có plan nào được nạp. Không phải lỗi — chỉ là chưa tới lúc tính lệnh.
    return false;
  }

  robot_nav_2d_msgs::Pose2DStamped pose;
  if (!currentPose(pose))
  {
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: pose lost, not computing a "
                              "command.\n");
    return false;
  }

  robot_geometry_msgs::Twist result;

  try
  {
    // Đo đúng lời gọi vào plugin, không đo cả hàm: phần còn lại (tra pose, kiểm NaN) là chi phí của
    // move_base2, còn đây mới là chi phí của local planner đang cấu hình.
    ScopedSection timer(stats_, section_compute_);
    // Gen-2 trả THẲNG lệnh (không có cờ thành công/thất bại) và ném exception khi không tính được —
    // ngược với gen-1. Vì vậy nhánh "không có lệnh hợp lệ" ở đây là nhánh catch.
    const robot_nav_2d_msgs::Twist2DStamped cmd_2d =
        active_->computeVelocityCommands(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_));
    result = robot_nav_2d_utils::twist2Dto3D(cmd_2d.velocity);
  }
  catch (const std::exception& ex)
  {
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: '%s' produced no command: %s\n",
                              active_name_.c_str(), ex.what());
    return false;
  }

  if (!isFiniteTwist(result))
  {
    // VelocityArbiter cũng chặn NaN/Inf, nhưng chặn ngay tại nguồn cho biết ĐÚNG plugin nào đang
    // trả dữ liệu hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: '%s' returned a command containing "
                              "NaN/Inf.\n",
                              active_name_.c_str());
    return false;
  }

  cmd = result;
  return true;
}

bool ControllerRunner::isGoalReached()
{
  if (!configured_ || active_ == nullptr)
  {
    return false;
  }

  if (!has_active_goal_)
  {
    return false;
  }

  robot_nav_2d_msgs::Pose2DStamped pose;
  if (!currentPose(pose))
  {
    return false;  // Mất pose: "chưa tới đích" là phía an toàn.
  }

  try
  {
    const bool reached =
        active_->isGoalReached(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_));
    if (reached)
    {
      has_active_goal_ = false;
      active_plan_.clear();
    }
    return reached;
  }
  catch (const std::exception& ex)
  {
    // Trả false: "chưa tới đích" là phía an toàn — báo nhầm đã tới sẽ kết thúc chặng đường sớm và
    // robot dừng ở chỗ không phải đích.
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: '%s' threw an exception in "
                              "isGoalReached: %s\n", active_name_.c_str(), ex.what());
    return false;
  }
}

void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
{
  plan = robot_nav_2d_msgs::Path2D();

  if (!configured_ || active_ == nullptr)
  {
    return;
  }

  try
  {
    ScopedSection timer(stats_, section_local_plan_);
    active_->getPlan(plan);
  }
  catch (const std::exception& ex)
  {
    // Không phải mọi planner đều hỗ trợ; gen-2 cho phép ném. Đây chỉ là dữ liệu hiển thị nên nuốt
    // exception là đúng — nhưng vẫn log để không ai tưởng rviz đang hiện quỹ đạo thật.
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: '%s' could not return a local "
                              "trajectory: %s\n", active_name_.c_str(), ex.what());
    plan = robot_nav_2d_msgs::Path2D();
  }
}

void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity)
{
  if (!isFiniteTwist(velocity))
  {
    // Giữ giá trị cũ thay vì đưa NaN vào hàm tính lệnh — nhiều local planner dùng nó làm mốc giới
    // hạn gia tốc, và NaN ở đó lan ra toàn bộ cost function.
    robot::log_error_throttle(kHotPathLogThrottle,
                              "[move_base2] ControllerRunner: dropping a measured velocity "
                              "containing NaN/Inf.\n");
    return;
  }
  measured_velocity_ = velocity;
}

bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear)
{
  if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z))
  {
    robot::log_error("[move_base2] ControllerRunner: linear velocity limit contains NaN/Inf, "
                     "ignored.\n");
    return false;
  }

  // Dấu chọn chiều — xem doc của ControllerPort::setTwistLinear. Nhớ cả hai chiều riêng để
  // swapPlanner còn áp lại được lên instance mới.
  if (linear.x < 0.0)
  {
    limit_linear_backward_ = linear;
    has_limit_linear_backward_ = true;
  }
  else
  {
    limit_linear_forward_ = linear;
    has_limit_linear_forward_ = true;
  }

  if (active_ == nullptr)
  {
    // Host đặt trần trước khi controller được nạp là chuyện bình thường: thứ tự khởi tạo không do
    // move_base2 quyết. Đã nhớ lại, sẽ áp khi có controller.
    return true;
  }

  try
  {
    return active_->setTwistLinear(linear);
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistLinear: "
                     "%s\n",
                     active_name_.c_str(), ex.what());
    return false;
  }
}

bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angular)
{
  if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z))
  {
    robot::log_error("[move_base2] ControllerRunner: angular velocity limit contains NaN/Inf, "
                     "ignored.\n");
    return false;
  }

  limit_angular_ = angular;
  has_limit_angular_ = true;

  if (active_ == nullptr)
  {
    return true;
  }

  try
  {
    return active_->setTwistAngular(angular);
  }
  catch (const std::exception& ex)
  {
    robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistAngular: "
                     "%s\n",
                     active_name_.c_str(), ex.what());
    return false;
  }
}

std::string ControllerRunner::activeController() const
{
  return active_ != nullptr ? active_name_ : std::string();
}

}  // namespace move_base2