cmake_minimum_required(VERSION 3.0.2)
project(recovery_core VERSION 0.1.0 LANGUAGES CXX)

# ========================================================
# Build mode detection
# ========================================================
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building recovery_core with Catkin")
else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building recovery_core with Standalone CMake")
endif()


# ========================================================
# C++ Standard
# ========================================================
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)


# ========================================================
# Common dependencies
# ========================================================
find_package(Boost REQUIRED COMPONENTS
  system
  thread
  filesystem
)

find_package(Threads REQUIRED)
find_package(yaml-cpp REQUIRED)


# ========================================================
# Standalone configuration
# ========================================================
if(NOT BUILDING_WITH_CATKIN)

  # Enable Position Independent Code
  set(CMAKE_POSITION_INDEPENDENT_CODE ON)

  # Runtime search path configuration
  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")


  # ------------------------------------------------------
  # Navigation source tree
  # ------------------------------------------------------
  set(PNKX_NAV_CORE_SRC_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../.."
  )

  # Test/recovery_core -> Test -> AMR_T800 -> src -> <workspace>. Bốn cấp, không phải bảy: bảy là
  # di sản từ vị trí cũ của gói và trỏ vào một thư mục không tồn tại.
  set(WORKSPACE_DEVEL_LIB_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../../../devel/lib"
  )


  # ------------------------------------------------------
  # Collect standalone package include directories
  # ------------------------------------------------------
  file(GLOB STANDALONE_PACKAGE_INCLUDE_DIRS
    LIST_DIRECTORIES true

    ${PNKX_NAV_CORE_SRC_DIR}/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/*/include
  )


  # robot_costmap_2d headers may expose PCL dependencies
  find_package(PCL QUIET COMPONENTS
    common
    io
  )


  set(STANDALONE_INCLUDE_DIRS
    ${STANDALONE_PACKAGE_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}

    /opt/ros/noetic/include
    /usr/local/include
  )


  if(PCL_FOUND)
    add_definitions(${PCL_DEFINITIONS})
  endif()


  # ------------------------------------------------------
  # Standalone libraries
  # ------------------------------------------------------
  set(PACKAGES_DIR
    robot_costmap_2d
    robot_cpp
    robot_time
    robot_xmlrpcpp
  )


  # ------------------------------------------------------
  # TF3
  # ------------------------------------------------------
  find_library(TF3_LIBRARY
    NAMES tf3

    PATHS
      /usr/lib
      /usr/local/lib
      /usr/lib/x86_64-linux-gnu
  )


  # ------------------------------------------------------
  # Library search paths
  # ------------------------------------------------------
  if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
    link_directories(${WORKSPACE_DEVEL_LIB_DIR})
  endif()

  link_directories(
    /usr/local/lib
  )


# ========================================================
# Catkin configuration
# ========================================================
else()

  # nav_test_harness chỉ dùng cho test (fake clock/costmap/pose/collision), nên nó KHÔNG nằm trong
  # catkin_package(CATKIN_DEPENDS) — consumer của recovery_core không phải kéo theo nó.
  find_package(catkin REQUIRED COMPONENTS
    robot_costmap_2d
    robot_cpp
    robot_time
    robot_geometry_msgs
    robot_nav_msgs
    robot_xmlrpcpp
    nav_test_harness
  )


  find_library(TF3_LIBRARY
    NAMES tf3

    PATHS
      /usr/lib
      /usr/local/lib
      /usr/lib/x86_64-linux-gnu
  )


  catkin_package(
    INCLUDE_DIRS
      include

    LIBRARIES
      recovery_core
      recovery_core_wait_recovery
      recovery_core_clear_costmap_recovery
      recovery_core_rotate_recovery
      recovery_core_back_up_recovery

    CATKIN_DEPENDS
      robot_costmap_2d
      robot_cpp
      robot_time
      robot_geometry_msgs
      robot_nav_msgs
      robot_xmlrpcpp

    DEPENDS
      Boost
  )


  include_directories(
    include
  )

  # Header của dependency vào dạng SYSTEM: gói này build với -Wall -Wextra, mà robot_cpp/costmap_2d
  # sinh hàng trăm warning riêng của chúng — để lẫn vào thì warning của chính gói này vô hình.
  include_directories(SYSTEM
    ${catkin_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${TF3_INCLUDE_DIR}
  )

endif()


# ========================================================
# Core library
# ========================================================
add_library(recovery_core SHARED
  src/recovery_types.cpp
  src/recovery_behavior.cpp
  src/recovery_registry.cpp

  # Adapter nối cổng của contract vào costmap thật. Nằm trong lib chính (không phải plugin) vì
  # host dựng chúng trực tiếp, không nạp qua Boost.DLL.
  adapters/costmap_pose_provider.cpp
  adapters/costmap_collision_checker.cpp
)

target_compile_options(recovery_core PRIVATE -Wall -Wextra)


# ========================================================
# Core library - Catkin
# ========================================================
if(BUILDING_WITH_CATKIN)

  add_dependencies(
    recovery_core

    ${${PROJECT_NAME}_EXPORTED_TARGETS}
    ${catkin_EXPORTED_TARGETS}
  )


  target_include_directories(recovery_core
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>

      ${Boost_INCLUDE_DIRS}
      ${TF3_INCLUDE_DIR}
  )


  target_link_libraries(recovery_core
    PUBLIC
      ${catkin_LIBRARIES}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::thread
      Boost::filesystem

      yaml-cpp

      Threads::Threads

      ${CMAKE_DL_LIBS}

      ${TF3_LIBRARY}
  )


# ========================================================
# Core library - Standalone
# ========================================================
else()

  target_include_directories(recovery_core
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>

    PRIVATE
      ${STANDALONE_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${TF3_INCLUDE_DIR}
  )


  target_link_libraries(recovery_core
    PUBLIC
      ${PACKAGES_DIR}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::thread
      Boost::filesystem

      yaml-cpp

      Threads::Threads

      ${CMAKE_DL_LIBS}

      ${TF3_LIBRARY}
  )


  set_target_properties(recovery_core PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY
      ${CMAKE_BINARY_DIR}

    BUILD_RPATH
      "${CMAKE_BINARY_DIR}"

    INSTALL_RPATH
      "${CMAKE_INSTALL_PREFIX}/lib"
  )

endif()


# ========================================================
# Recovery plugin helper
# ========================================================
set(RECOVERY_CORE_PLUGIN_TARGETS)


function(add_recovery_core_plugin target source)

  add_library(${target} SHARED
    ${source}
  )


  target_compile_options(${target} PRIVATE -Wall -Wextra)


  add_dependencies(
    ${target}
    recovery_core
  )


  # ======================================================
  # Catkin plugin
  # ======================================================
  if(BUILDING_WITH_CATKIN)

    add_dependencies(
      ${target}

      ${${PROJECT_NAME}_EXPORTED_TARGETS}
      ${catkin_EXPORTED_TARGETS}
    )


    target_include_directories(${target}
      PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
    )


    target_link_libraries(${target}
      PUBLIC
        recovery_core

      PRIVATE
        ${catkin_LIBRARIES}

        Boost::boost
        Boost::system
        Boost::thread
        Boost::filesystem

        yaml-cpp

        Threads::Threads

        ${CMAKE_DL_LIBS}

        ${TF3_LIBRARY}
    )


  # ======================================================
  # Standalone plugin
  # ======================================================
  else()

    target_include_directories(${target}
      PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>

      PRIVATE
        ${STANDALONE_INCLUDE_DIRS}
        ${TF3_INCLUDE_DIR}
    )


    target_link_libraries(${target}
      PUBLIC
        recovery_core

      PRIVATE
        Boost::boost
        Boost::system
        Boost::thread
        Boost::filesystem

        yaml-cpp

        Threads::Threads

        ${CMAKE_DL_LIBS}

        ${TF3_LIBRARY}
    )


    set_target_properties(${target} PROPERTIES
      LIBRARY_OUTPUT_DIRECTORY
        ${CMAKE_BINARY_DIR}

      BUILD_RPATH
        "${CMAKE_BINARY_DIR}"

      INSTALL_RPATH
        "${CMAKE_INSTALL_PREFIX}/lib"
    )

  endif()


  set_target_properties(${target} PROPERTIES
    POSITION_INDEPENDENT_CODE ON
  )


  list(APPEND
    RECOVERY_CORE_PLUGIN_TARGETS
    ${target}
  )


  set(
    RECOVERY_CORE_PLUGIN_TARGETS
    ${RECOVERY_CORE_PLUGIN_TARGETS}
    PARENT_SCOPE
  )

endfunction()


# ========================================================
# Recovery plugins
# ========================================================
# Bộ recovery DEFAULT của gói. Thứ tự chạy do YAML quyết định (mục 3.4 của NAV_REFACTOR_PLAN.md),
# không phải thứ tự khai báo ở đây.
add_recovery_core_plugin(
  recovery_core_wait_recovery
  plugins/wait_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_clear_costmap_recovery
  plugins/clear_costmap_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_rotate_recovery
  plugins/rotate_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_back_up_recovery
  plugins/back_up_recovery.cpp
)


# ========================================================
# Install - Catkin
# ========================================================
if(BUILDING_WITH_CATKIN)

  install(
    TARGETS
      recovery_core
      ${RECOVERY_CORE_PLUGIN_TARGETS}

    ARCHIVE DESTINATION
      ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION
      ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION
      ${CATKIN_GLOBAL_BIN_DESTINATION}
  )


  install(
    DIRECTORY
      include/${PROJECT_NAME}/

    DESTINATION
      ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

    FILES_MATCHING
      PATTERN "*.h"
      PATTERN "*.hpp"

    PATTERN ".svn" EXCLUDE
  )


# ========================================================
# Install - Standalone
# ========================================================
else()

  install(
    TARGETS
      recovery_core
      ${RECOVERY_CORE_PLUGIN_TARGETS}

    EXPORT
      ${PROJECT_NAME}-targets

    ARCHIVE DESTINATION
      lib

    LIBRARY DESTINATION
      lib

    RUNTIME DESTINATION
      bin
  )


  install(
    EXPORT
      ${PROJECT_NAME}-targets

    FILE
      ${PROJECT_NAME}-targets.cmake

    NAMESPACE
      ${PROJECT_NAME}::

    DESTINATION
      lib/cmake/${PROJECT_NAME}
  )


  install(
    DIRECTORY
      include/${PROJECT_NAME}/

    DESTINATION
      include

    FILES_MATCHING
      PATTERN "*.h"
      PATTERN "*.hpp"

    PATTERN ".svn" EXCLUDE
  )


  # ------------------------------------------------------
  # Print configuration information
  # ------------------------------------------------------
  message(STATUS "=================================")
  message(STATUS "Project: ${PROJECT_NAME}")
  message(STATUS "Version: ${PROJECT_VERSION}")
  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
  message(STATUS "Libraries:")
  message(STATUS "  recovery_core")
  message(STATUS "  recovery_core_wait_recovery")
  message(STATUS "  recovery_core_clear_costmap_recovery")
  message(STATUS "  recovery_core_rotate_recovery")
  message(STATUS "  recovery_core_back_up_recovery")
  message(STATUS "Dependencies:")
  message(STATUS "  robot_costmap_2d")
  message(STATUS "  robot_cpp")
  message(STATUS "  robot_time")
  message(STATUS "  robot_geometry_msgs")
  message(STATUS "  robot_nav_msgs")
  message(STATUS "  robot_xmlrpcpp")
  message(STATUS "  tf3")
  message(STATUS "  Boost")
  message(STATUS "  yaml-cpp")
  message(STATUS "=================================")

endif()


# ========================================================
# Tests
# ========================================================
# Đăng ký qua catkin_add_gtest để `catkin_make run_tests` / `ctest` bắt được, đồng thời vẫn chạy
# trực tiếp được binary trong devel/lib/recovery_core.
if(CATKIN_ENABLE_TESTING AND BUILDING_WITH_CATKIN)

  if(NOT COMMAND catkin_add_gtest)
    message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
  endif()

  set(RECOVERY_CORE_TESTS
    recovery_lifecycle_test
    output_kind_test
    goal_semantics_test
    timeout_test
    wait_recovery_test
    backup_safety_test
    pose_progress_test
    rotate_safety_test
    registry_test
  )

  foreach(test_name ${RECOVERY_CORE_TESTS})
    catkin_add_gtest(${test_name} test/${test_name}.cpp)

    if(TARGET ${test_name})
      set_target_properties(${test_name} PROPERTIES EXCLUDE_FROM_ALL FALSE)

      target_compile_options(${test_name} PRIVATE -Wall -Wextra)

      target_include_directories(${test_name}
        PRIVATE
          ${CMAKE_CURRENT_SOURCE_DIR}/include
          ${CMAKE_CURRENT_SOURCE_DIR}/test
      )

      target_include_directories(${test_name} SYSTEM
        PRIVATE
          ${catkin_INCLUDE_DIRS}
      )

      # catkin_add_gtest dùng target_link_libraries dạng plain, nên phần bổ sung cũng phải plain.
      # Plugin KHÔNG được link thẳng: chúng chỉ export alias Boost.DLL và được nạp qua
      # RecoveryRegistry, đúng đường mà runtime đi.
      target_link_libraries(${test_name}
        recovery_core
        ${catkin_LIBRARIES}
        ${Boost_LIBRARIES}
        yaml-cpp
        pthread
        ${CMAKE_DL_LIBS}
      )

      add_dependencies(${test_name} ${RECOVERY_CORE_PLUGIN_TARGETS})

      # Đường tới cây config test và thư mục .so, để binary tự trỏ đúng chỗ khi chạy qua ctest
      # (ctest không mang theo PNKX_NAV_CORE_CONFIG_DIR hay LD_LIBRARY_PATH của shell).
      target_compile_definitions(${test_name} PRIVATE
        RECOVERY_CORE_TEST_CONFIG_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/config"
        RECOVERY_CORE_TEST_LIBRARY_DIR="${CATKIN_DEVEL_PREFIX}/lib"
      )
    endif()
  endforeach()

endif()