cmake_minimum_required(VERSION 3.0.2)
project(move_base2 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 move_base2 with Catkin")
else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building move_base2 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)

  set(CMAKE_POSITION_INDEPENDENT_CODE ON)

  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")

  # Test/<pkg> -> src/AMR_T800/pnkx_nav_core/src
  set(PNKX_NAV_CORE_SRC_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../pnkx_nav_core/src"
  )

  # Test/<pkg> -> T800_ws/devel/lib
  set(WORKSPACE_DEVEL_LIB_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../../../devel/lib"
  )

  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
  )

  find_package(PCL QUIET COMPONENTS common io)

  set(STANDALONE_INCLUDE_DIRS
    ${STANDALONE_PACKAGE_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}

    # These runtime packages are siblings while they still live below Test/.
    # The explicit paths keep `cmake ..` in move_base2 working before the
    # packages are moved below pnkx_nav_core/src.
    ${CMAKE_CURRENT_SOURCE_DIR}/../recovery_core/include
    ${CMAKE_CURRENT_SOURCE_DIR}/../action_core/include
    ${CMAKE_CURRENT_SOURCE_DIR}/../mission_adapters/include
    ${CMAKE_CURRENT_SOURCE_DIR}/../nav_test_harness/include

    /usr/local/include
  )

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

  set(PACKAGES_DIR
    robot_costmap_2d
    robot_nav_2d_utils
    robot_cpp
    robot_time
    robot_xmlrpcpp

    # move_base2 public headers and runners use these packages directly.
    # When configured from the pnkx_nav_core root, these names resolve to
    # CMake targets and propagate their public include directories.
    recovery_core
    action_core
    mission_adapters
    nav_test_harness
  )

  find_library(TF3_LIBRARY
    NAMES tf3
    PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
  )

  if(NOT TF3_LIBRARY)
    message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
  endif()

  if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
    link_directories(${WORKSPACE_DEVEL_LIB_DIR})
  endif()

  link_directories(/usr/local/lib)

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

  find_package(catkin REQUIRED COMPONENTS
    move_base_core
    robot_nav_core
    robot_costmap_2d
    robot_cpp
    robot_time
    robot_geometry_msgs
    robot_std_msgs
    robot_nav_msgs
    robot_nav_2d_msgs
    robot_nav_2d_utils
    robot_sensor_msgs
    robot_map_msgs
    robot_protocol_msgs
    robot_xmlrpcpp

    # SensorGateway là chỗ duy nhất trong gói này lọc laser trước khi vào costmap.
    laser_filter

    # RecoveryRunner include thẳng recovery_core: đó là chỗ duy nhất trong gói này biết tới nó.
    recovery_core

    # ActionRunner include thẳng action_core: chỗ duy nhất trong gói này biết tới nó.
    action_core

    # `bridges/` include thẳng mission_adapters: biên duy nhất trong gói này biết tới nó
    # (MissionAdapterBridge dịch contract, MissionLayer lắp ráp framework).
    mission_adapters

    # scenario_test chạy kịch bản khai báo qua khung của nav_test_harness.
    nav_test_harness
  )

  find_library(TF3_LIBRARY
    NAMES tf3
    PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
  )

  if(NOT TF3_LIBRARY)
    message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
  endif()

  catkin_package(
    INCLUDE_DIRS
      include

    LIBRARIES
      move_base2_core
      move_base2

    CATKIN_DEPENDS
      move_base_core
      robot_nav_core
      robot_costmap_2d
      robot_cpp
      robot_time
      robot_geometry_msgs
      robot_std_msgs
      robot_nav_msgs
      robot_nav_2d_msgs
      robot_nav_2d_utils
      robot_sensor_msgs
      robot_map_msgs
      robot_protocol_msgs
      robot_xmlrpcpp

    DEPENDS
      Boost
  )

  include_directories(include)

  # Dependency vào dạng SYSTEM: `robot/robot.h` kéo theo `console.h`, file này khai vài chục hằng màu
  # ở namespace scope và sinh đúng số đó warning -Wunused-variable. Để chúng lẫn vào output sẽ chôn
  # mất warning THẬT của gói — mà cờ -Wall -Wextra ở dưới có mặt chính là để thấy những warning đó.
  include_directories(SYSTEM
    ${catkin_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
  )

endif()


# ========================================================
# Core library — logic thuần, không I/O.
#
# Tách riêng khỏi plugin để test lõi không phải kéo theo contract host, và để bất kỳ ai muốn nhúng
# state machine vào runtime khác cũng dùng lại được.
# ========================================================
add_library(move_base2_core SHARED
  src/navigation_state.cpp
  src/state_machine.cpp
  src/velocity_arbiter.cpp
  src/control_loop.cpp
  src/config/move_base2_config.cpp
  src/runners/recovery_runner.cpp
  src/runners/action_runner.cpp
  src/runners/planner_runner.cpp
  src/runners/controller_runner.cpp
  src/io/runtime_stats.cpp
  src/io/sensor_gateway.cpp
  src/io/costmap_exporter.cpp
  src/bridges/mission_adapter_bridge.cpp
  src/bridges/mission_layer.cpp
  src/navigation_runtime.cpp
)

# Gói này KHÔNG kế thừa cờ -w của pnkx_nav_core: warning ở đây phải nhìn thấy được.
target_compile_options(move_base2_core PRIVATE -Wall -Wextra)

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


# ========================================================
# Plugin library — facade BaseNavigation + export Boost.DLL.
# ========================================================
add_library(move_base2 SHARED
  src/navigation_server.cpp
  src/move_base2_plugin.cpp
)

target_compile_options(move_base2 PRIVATE -Wall -Wextra)

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

set_target_properties(move_base2 PROPERTIES POSITION_INDEPENDENT_CODE ON)


# ========================================================
# Linking
# ========================================================
if(BUILDING_WITH_CATKIN)

  add_dependencies(move_base2_core
    ${${PROJECT_NAME}_EXPORTED_TARGETS}
    ${catkin_EXPORTED_TARGETS}
  )

  add_dependencies(move_base2
    move_base2_core
    ${${PROJECT_NAME}_EXPORTED_TARGETS}
    ${catkin_EXPORTED_TARGETS}
  )

  target_link_libraries(move_base2_core
    PUBLIC
      ${catkin_LIBRARIES}

    PRIVATE
      Boost::boost
      Threads::Threads
      ${TF3_LIBRARY}
  )

  target_link_libraries(move_base2
    PUBLIC
      move_base2_core
      ${catkin_LIBRARIES}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::filesystem
      Threads::Threads
      ${CMAKE_DL_LIBS}
      ${TF3_LIBRARY}
  )

else()

  target_include_directories(move_base2_core
    PRIVATE
      ${STANDALONE_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
  )

  target_include_directories(move_base2
    PRIVATE
      ${STANDALONE_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
  )

  target_link_libraries(move_base2_core
    PUBLIC
      ${PACKAGES_DIR}

    PRIVATE
      Boost::boost
      Threads::Threads
      ${TF3_LIBRARY}
  )

  target_link_libraries(move_base2
    PUBLIC
      move_base2_core
      ${PACKAGES_DIR}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::filesystem
      Threads::Threads
      ${CMAKE_DL_LIBS}
      ${TF3_LIBRARY}
  )

  set_target_properties(move_base2_core move_base2 PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
    BUILD_RPATH "${CMAKE_BINARY_DIR}"
    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
  )

endif()


# ========================================================
# Install
# ========================================================
if(BUILDING_WITH_CATKIN)

  install(TARGETS move_base2_core move_base2
    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"
  )

  install(DIRECTORY launch config
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
  )

else()

  install(TARGETS move_base2_core move_base2
    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"
  )

endif()


# ========================================================
# Tests
# ========================================================
option(BUILD_MOVE_BASE2_TESTS "Build move_base2 tests" ON)

if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN)

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

  # Plugin global planner chỉ dùng cho test — nạp qua Boost.DLL đúng đường runtime đi, không link
  # thẳng. Đặt trong nhánh test để không bao giờ lọt vào bản cài đặt.
  foreach(test_plugin move_base2_test_global_planner move_base2_test_local_planner)
    if(test_plugin STREQUAL "move_base2_test_global_planner")
      set(test_plugin_src test/plugins/test_global_planner.cpp)
    else()
      set(test_plugin_src test/plugins/test_local_planner.cpp)
    endif()

    add_library(${test_plugin} SHARED ${test_plugin_src})

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

    target_include_directories(${test_plugin}
      PRIVATE
        ${CMAKE_CURRENT_SOURCE_DIR}/include
    )

    set_target_properties(${test_plugin} PROPERTIES POSITION_INDEPENDENT_CODE ON)

    target_link_libraries(${test_plugin}
      PRIVATE
        ${catkin_LIBRARIES}
        Boost::boost
        ${CMAKE_DL_LIBS}
    )
  endforeach()

  set(MOVE_BASE2_TESTS
    state_machine_test
    velocity_arbiter_test
    walking_skeleton_test
    config_validation_test
    action_runner_test
    recovery_runner_test
    sensor_gateway_test
    runtime_stats_test
    navigation_server_test
    planner_runner_test
    controller_runner_test
    mission_adapter_bridge_test
    mission_layer_test
    move_base2_scenario_test   # tên có tiền tố gói: nav_test_harness đã có target scenario_test
  )

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

    if(TARGET ${test_name})
      # catkin_add_gtest đặt target EXCLUDE_FROM_ALL; mở ra để binary có mặt sau `catkin_make`
      # thường, đúng quy trình verify của repo (chạy thẳng ./devel/lib/<pkg>/<test>).
      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
        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.
      target_link_libraries(${test_name}
        move_base2_core
        ${catkin_LIBRARIES}
        ${Boost_LIBRARIES}
        yaml-cpp
        ${CMAKE_DL_LIBS}
      )

      # Plugin recovery được nạp qua Boost.DLL từ devel/lib, không link thẳng: đúng đường runtime đi.
      # ctest không mang theo PNKX_NAV_CORE_* của shell nên binary tự trỏ.
      target_compile_definitions(${test_name} PRIVATE
        MOVE_BASE2_TEST_CONFIG_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/config"
        MOVE_BASE2_TEST_LIBRARY_DIR="${CATKIN_DEVEL_PREFIX}/lib"
        # Kịch bản là DỮ LIỆU nằm ở nav_test_harness; test không được đoán đường dẫn tới nó.
        MOVE_BASE2_SCENARIO_DIR="${CMAKE_CURRENT_SOURCE_DIR}/../nav_test_harness/scenarios"
      )
    endif()
  endforeach()

  # `NavigationServer` nằm trong thư viện plugin `move_base2`, không phải trong `move_base2_core`.
  if(TARGET navigation_server_test)
    target_link_libraries(navigation_server_test move_base2)
  endif()

  # Plugin phải có mặt trong devel/lib trước khi test chạy — nó được nạp qua Boost.DLL lúc chạy.
  if(TARGET planner_runner_test)
    add_dependencies(planner_runner_test move_base2_test_global_planner)
  endif()

  if(TARGET controller_runner_test)
    add_dependencies(controller_runner_test move_base2_test_local_planner)
  endif()

endif()