/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Plugin Loader Helper for Boost.DLL
 * Implementation
 *********************************************************************/
#include <robot/plugin_loader_helper.h>
#include <robot/console.h>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <filesystem>
#include <dlfcn.h>
#include <link.h>
#include <unistd.h>
#include <limits.h>
#ifdef __linux__
#include <linux/limits.h>
#endif

// CMAKE_BINARY_DIR được define trong CMakeLists.txt
#ifndef PNKX_NAV_CORE_BUILD_DIR
#define PNKX_NAV_CORE_BUILD_DIR ""
#endif

namespace robot
{

PluginLoaderHelper::PluginLoaderHelper()
  : PluginLoaderHelper(robot::NodeHandle(), "")
{
  // Delegate to the other constructor with default NodeHandle
}

PluginLoaderHelper::PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace)
  : nh_(nh), config_namespace_(config_namespace)
{
  std::string build_dir = getBuildDirectory();
  if (!build_dir.empty()) {
    search_paths_.push_back(build_dir);
  }
  
  // Thêm các subdirectories thường dùng từ workspace path
  std::string workspace_path = getWorkspacePath();
  if (!workspace_path.empty()) {
    search_paths_.push_back(workspace_path + "/install/lib");
    search_paths_.push_back(workspace_path + "/devel/lib");
  }
}

void PluginLoaderHelper::setNodeHandle(robot::NodeHandle nh, const std::string& config_namespace)
{
  nh_ = nh;
  config_namespace_ = config_namespace;
}

std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
{
  // Build parameter path: {config_namespace}/{symbol_name}/library_path
  std::string param_path;
  if (!config_namespace_.empty()) {
    param_path = config_namespace_ + "/" + symbol_name + "/library_path";
  } else {
    param_path = symbol_name + "/library_path";
  }
  // Try to read from NodeHandle
  std::string library_path;
  robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
  if (nh_.hasParam(param_path)) {
    nh_.getParam(param_path, library_path, std::string(""));
    if (!library_path.empty()) {
      std::string resolved_path = resolveLibraryPath(library_path);
      if (!resolved_path.empty()) {
        return resolved_path;
      }
    }
  }
  
  // Try without namespace in symbol name (e.g., "custom_planner::CustomPlanner" -> "CustomPlanner")
  size_t ns_pos = symbol_name.find_last_of("::");
  if (ns_pos != std::string::npos) {
    std::string short_name = symbol_name.substr(ns_pos + 1);
    std::string short_param_path;
    if (!config_namespace_.empty()) {
      short_param_path = config_namespace_ + "/" + short_name + "/library_path";
    } else {
      short_param_path = short_name + "/library_path";
    }
    
    if (nh_.hasParam(short_param_path)) {
      nh_.getParam(short_param_path, library_path, std::string(""));
      if (!library_path.empty()) {
        std::string resolved_path = resolveLibraryPath(library_path);
        if (!resolved_path.empty()) {
          return resolved_path;
        }
      }
    }
  }
  // Try LD_LIBRARY_PATH as fallback
  const char* ld_path = std::getenv("LD_LIBRARY_PATH");
  if (ld_path) {
    std::string ld_path_str(ld_path);
    std::stringstream ss(ld_path_str);
    std::string path;

    while (std::getline(ss, path, ':')) {
      if (!path.empty()) {
        // Try different naming conventions
        std::vector<std::string> possible_names = {
          "lib" + library_path + ".so",
          library_path + ".so"
        };
        
        // Also try without namespace
        if (ns_pos != std::string::npos) {
          std::string short_name = symbol_name.substr(ns_pos + 1);
          possible_names.push_back("lib" + short_name + ".so");
          possible_names.push_back(short_name + ".so");
        }
      
        for (const auto& name : possible_names) {
          std::filesystem::path full_path = std::filesystem::path(path) / name;
          if (std::filesystem::exists(full_path)) {
            log_info("[PluginLoaderHelper] Found library in LD_LIBRARY_PATH: %s", 
                     full_path.string().c_str());
            return full_path.string();
          }
        }
      }
    }
  }

  log_warning("[PluginLoaderHelper] Library not found for symbol: %s", symbol_name.c_str());
  return "";
}

bool PluginLoaderHelper::hasSymbol(const std::string& symbol_name) const
{
  // Build parameter path: {config_namespace}/{symbol_name}/library_path
  std::string param_path;
  if (!config_namespace_.empty()) {
    param_path = config_namespace_ + "/" + symbol_name + "/library_path";
  } else {
    param_path = symbol_name + "/library_path";
  }
  
  if (nh_.hasParam(param_path)) {
    return true;
  }
  
  // Check without namespace
  size_t ns_pos = symbol_name.find_last_of("::");
  if (ns_pos != std::string::npos) {
    std::string short_name = symbol_name.substr(ns_pos + 1);
    std::string short_param_path;
    if (!config_namespace_.empty()) {
      short_param_path = config_namespace_ + "/" + short_name + "/library_path";
    } else {
      short_param_path = short_name + "/library_path";
    }
    return nh_.hasParam(short_param_path);
  }
  
  return false;
}

std::vector<std::string> PluginLoaderHelper::getRegisteredSymbols() const
{
  // Note: This is a simplified implementation. 
  // To get all registered symbols, we would need to iterate through NodeHandle's parameter tree.
  // For now, return empty vector as this requires more complex implementation.
  // Users should know which symbols are available from their config files.
  return std::vector<std::string>();
}

std::string PluginLoaderHelper::resolveLibraryPath(const std::string& library_path) const
{
  // If absolute path and exists, return it directly
  if (std::filesystem::path(library_path).is_absolute()) {
    if (std::filesystem::exists(library_path)) {
      try {
        return std::filesystem::canonical(library_path).string();
      } catch (...) {
        return library_path;
      }
    }
    // Try to resolve (follow symlinks)
    try {
      std::filesystem::path resolved = std::filesystem::canonical(library_path);
      if (std::filesystem::exists(resolved)) {
        return resolved.string();
      }
    } catch (...) {
      // Ignore errors
    }
    return "";
  }

  // If relative path, search under PNKX_NAV_CORE_LIBRARY_PATH (':'-separated, like LD_LIBRARY_PATH)
  const char* nav_lib_path_env = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
  if (nav_lib_path_env) {
    std::string lib_path_with_ext = library_path;
    if (lib_path_with_ext.find(".so") == std::string::npos) {
      lib_path_with_ext += ".so";
    }
    std::string nav_lib_paths(nav_lib_path_env);
    std::stringstream ss(nav_lib_paths);
    std::string base_dir;
    while (std::getline(ss, base_dir, ':')) {
      if (base_dir.empty()) {
        continue;
      }
      std::filesystem::path full_path = std::filesystem::path(base_dir) / lib_path_with_ext;
      if (std::filesystem::exists(full_path)) {
        try {
          return std::filesystem::canonical(full_path).string();
        } catch (...) {
          return full_path.string();
        }
      }
    }
  }

  // Try in search_paths
  for (const auto& base_path : search_paths_) {
    std::filesystem::path full_path = std::filesystem::path(base_path) / library_path;
    if (std::filesystem::exists(full_path)) {
      try {
        return std::filesystem::canonical(full_path).string();
      } catch (...) {
        return full_path.string();
      }
    }
  }

  // Try in current directory
  if (std::filesystem::exists(library_path)) {
    try {
      return std::filesystem::canonical(library_path).string();
    } catch (...) {
      return std::filesystem::absolute(library_path).string();
    }
  }

  return "";
}

std::string PluginLoaderHelper::getBuildDirectory()
{
  // Method 1: Từ compile definition (CMAKE_BINARY_DIR)
  #ifdef PNKX_NAV_CORE_BUILD_DIR
  std::string build_dir = PNKX_NAV_CORE_BUILD_DIR;
  if (!build_dir.empty() && std::filesystem::exists(build_dir)) {
    return build_dir;
  }
  #endif

  // Method 2: Từ environment variable
  const char* env_build_dir = std::getenv("PNKX_NAV_CORE_BUILD_DIR");
  if (env_build_dir && std::filesystem::exists(env_build_dir)) {
    return std::string(env_build_dir);
  }

  // Method 3: Tự động detect từ library path (librobot_cpp.so)
  // Lấy đường dẫn của chính library này
  Dl_info dl_info;
  if (dladdr(reinterpret_cast<void*>(&PluginLoaderHelper::getBuildDirectory), &dl_info) != 0) {
    if (dl_info.dli_fname) {
      std::filesystem::path lib_path(dl_info.dli_fname);
      std::filesystem::path lib_dir = lib_path.parent_path();
      
      // Nếu library nằm trong build directory, lấy parent
      // Ví dụ: /path/to/build/librobot_cpp.so -> /path/to/build
      if (lib_dir.filename() == "lib" || lib_dir.filename().string().find("lib") == 0) {
        // Có thể là build/lib/ hoặc build/src/.../lib/
        std::filesystem::path parent = lib_dir.parent_path();
        if (std::filesystem::exists(parent) && std::filesystem::is_directory(parent)) {
          // Kiểm tra xem có phải build directory không (có CMakeCache.txt)
          if (std::filesystem::exists(parent / "CMakeCache.txt")) {
            return parent.string();
          }
        }
      } else {
        // Library nằm trực tiếp trong build directory
        if (std::filesystem::exists(lib_dir / "CMakeCache.txt")) {
          return lib_dir.string();
        }
      }
    }
  }

  // Method 4: Từ workspace path + /build
  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
  if (workspace_path) {
    std::string build_path = std::string(workspace_path) + "/build";
    if (std::filesystem::exists(build_path) && std::filesystem::is_directory(build_path)) {
      return build_path;
    }
  }

  // Method 5: Từ executable path (tương tự NodeHandle)
  try {
    char exe_path[PATH_MAX];
    ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
    if (count != -1) {
      exe_path[count] = '\0';
      std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();
      
      // Thử các relative paths: ../build, ../../build, etc.
      std::vector<std::filesystem::path> relative_paths = {
        exe_dir / "../build",
        exe_dir / "../../build",
        exe_dir.parent_path() / "build",
        exe_dir.parent_path().parent_path() / "build"
      };
      
      for (const auto& rel_path : relative_paths) {
        if (std::filesystem::exists(rel_path) && 
            std::filesystem::is_directory(rel_path) &&
            std::filesystem::exists(rel_path / "CMakeCache.txt")) {
          try {
            return std::filesystem::canonical(rel_path).string();
          } catch (...) {
            return rel_path.string();
          }
        }
      }
    }
  } catch (const std::exception&) {
    // Ignore errors
  }

  // Method 6: Hardcoded fallback
  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build";
  if (std::filesystem::exists(fallback)) {
    return fallback;
  }

  return "";
}

std::string PluginLoaderHelper::getWorkspacePath()
{
  // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
  const char* workspace_path = std::getenv("PNKX_NAV_CORE_LIBRARY_PATH");
  if (workspace_path && std::filesystem::exists(workspace_path)) {
    return std::string(workspace_path);
  }

  // Method 2: Từ environment variable ROS_WORKSPACE hoặc CATKIN_WS
  const char* ros_workspace = std::getenv("ROS_WORKSPACE");
  if (ros_workspace && std::filesystem::exists(ros_workspace)) {
    return std::string(ros_workspace);
  }

  const char* catkin_ws = std::getenv("CATKIN_WS");
  if (catkin_ws && std::filesystem::exists(catkin_ws)) {
    return std::string(catkin_ws);
  }

  // Method 3: Từ build directory (lấy parent directory)
  std::string build_dir = getBuildDirectory();
  if (!build_dir.empty()) {
    std::filesystem::path build_path(build_dir);
    std::filesystem::path parent = build_path.parent_path();
    // Kiểm tra xem có phải workspace không (có thư mục src/)
    if (std::filesystem::exists(parent / "src") && std::filesystem::is_directory(parent / "src")) {
      return parent.string();
    }
  }

  // Method 4: Từ executable path và tìm workspace
  try {
    char exe_path[PATH_MAX];
    ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
    if (count != -1) {
      exe_path[count] = '\0';
      std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();
      
      // Thử các relative paths: ../.., ../../.., etc. để tìm workspace
      std::vector<std::filesystem::path> relative_paths = {
        exe_dir.parent_path().parent_path(),  // ../..
        exe_dir.parent_path().parent_path().parent_path(),  // ../../..
        exe_dir.parent_path().parent_path().parent_path().parent_path()  // ../../../..
      };
      
      for (const auto& rel_path : relative_paths) {
        // Kiểm tra xem có phải workspace không (có thư mục src/ hoặc build/)
        if (std::filesystem::exists(rel_path) && 
            std::filesystem::is_directory(rel_path) &&
            (std::filesystem::exists(rel_path / "src") || 
             std::filesystem::exists(rel_path / "build"))) {
          try {
            return std::filesystem::canonical(rel_path).string();
          } catch (...) {
            return rel_path.string();
          }
        }
      }
    }
  } catch (const std::exception&) {
    // Ignore errors
  }

  // Method 5: Từ library path (librobot_cpp.so) và tìm workspace
  Dl_info dl_info;
  if (dladdr(reinterpret_cast<void*>(&PluginLoaderHelper::getWorkspacePath), &dl_info) != 0) {
    if (dl_info.dli_fname) {
      std::filesystem::path lib_path(dl_info.dli_fname);
      std::filesystem::path lib_dir = lib_path.parent_path();
      
      // Tìm workspace từ library path
      std::filesystem::path current = lib_dir;
      for (int i = 0; i < 5; ++i) {  // Tối đa 5 levels up
        if (std::filesystem::exists(current / "src") || 
            std::filesystem::exists(current / "build")) {
          try {
            return std::filesystem::canonical(current).string();
          } catch (...) {
            return current.string();
          }
        }
        current = current.parent_path();
        if (current == current.parent_path()) {  // Reached root
          break;
        }
      }
    }
  }

  // Method 6: Hardcoded fallback (nếu các phương pháp trên không hoạt động)
  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws";
  if (std::filesystem::exists(fallback)) {
    return fallback;
  }

  return "";
}

} // namespace robot

