# --- CMake version và project name ---
cmake_minimum_required(VERSION 3.10)
project(costmap_2d)

# --- C++ standard và position independent code ---
set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành shared lib

# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
# Dùng để runtime linker tìm thư viện đã build trước khi install
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)

# --- Dependencies ---
# Tìm các thư viện cần thiết
# find_package(tf3 REQUIRED)            # Nếu dùng tf3
find_package(Eigen3 REQUIRED)          # Thư viện Eigen cho toán học
find_package(Boost REQUIRED COMPONENTS system thread filesystem)  # Boost: system, thread, filesystem
find_package(GTest REQUIRED)           # Google Test cho unit test
find_package(PCL REQUIRED COMPONENTS common io)   # Point Cloud Library
find_package(xmlrpcpp REQUIRED)        # XML-RPC client/server library

# --- Include other message packages nếu cần ---
# Có thể add_subdirectory nếu các package ROS msgs không có target
# ví dụ sensor_msgs, geometry_msgs, nav_msgs,... 
# mục đích để build mà không cần cài ROS

# --- Define macro để dùng trong code ---
add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")

# --- Include directories ---
# Thêm các folder chứa header files
include_directories(
    include  
    ${EIGEN3_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})  # Thêm thư viện PCL vào linker path

# --- Eigen và PCL definitions ---
add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})

# --- Core library: costmap_2d ---
# Tạo thư viện chính
add_library(costmap_2d
    src/costmap_2d_robot.cpp
    src/array_parser.cpp
    src/costmap_2d.cpp
    src/observation_buffer.cpp
    src/layer.cpp
    src/layered_costmap.cpp
    src/costmap_math.cpp
    src/footprint.cpp
    src/costmap_layer.cpp
)

# --- Link các thư viện phụ thuộc ---
target_link_libraries(costmap_2d
    ${Boost_LIBRARIES}        # Boost
    std_msgs                   # ROS msgs
    sensor_msgs
    geometry_msgs
    nav_msgs
    map_msgs
    laser_geometry
    voxel_grid
    tf3
    tf3_geometry_msgs
    tf3_sensor_msgs
    data_convert
    xmlrpcpp                  # XMLRPC
    yaml-cpp
    dl
)

# --- Include directories cho target ---
target_include_directories(costmap_2d 
        PUBLIC 
            ${Boost_INCLUDE_DIRS}  # Boost headers
            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
            $<INSTALL_INTERFACE:include/${PROJECT_NAME}>             # Khi install
        )

# --- Cài đặt thư viện ---
install(TARGETS costmap_2d
  EXPORT costmap_2dTargets
  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
  LIBRARY DESTINATION lib      # Thư viện động .so
  RUNTIME DESTINATION bin      # File thực thi (nếu có)
  INCLUDES DESTINATION include # Cài đặt include
)

# --- Cài đặt headers ---
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION include/${PROJECT_NAME}
)

# # --- Export CMake targets ---
# install(EXPORT costmap_2dTargets
#   DESTINATION lib/cmake/costmap_2d
# )

# --- Plugin libraries ---
# Tạo các plugin shared library
add_library(static_layer SHARED plugins/static_layer.cpp)
target_link_libraries(static_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)

add_library(obstacle_layer SHARED plugins/obstacle_layer.cpp)
target_link_libraries(obstacle_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)

add_library(inflation_layer SHARED plugins/inflation_layer.cpp)
target_link_libraries(inflation_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)

add_library(voxel_layer SHARED plugins/voxel_layer.cpp)
target_link_libraries(voxel_layer costmap_2d ${Boost_LIBRARIES} yaml-cpp)

add_library(critical_layer SHARED plugins/critical_layer.cpp)
target_link_libraries(critical_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)

add_library(directional_layer SHARED plugins/directional_layer.cpp)
target_link_libraries(directional_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)

add_library(preferred_layer SHARED plugins/preferred_layer.cpp)
target_link_libraries(preferred_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)

add_library(unpreferred_layer SHARED plugins/unpreferred_layer.cpp)
target_link_libraries(unpreferred_layer costmap_2d static_layer ${Boost_LIBRARIES} yaml-cpp)


# --- Option để bật/tắt test ---
option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" OFF)

if(BUILD_COSTMAP_TESTS)
    # --- Test executables ---
    add_executable(test_array_parser test/array_parser_test.cpp)
    add_executable(test_costmap test/coordinates_test.cpp)
    add_executable(test_plugin test/static_layer_test.cpp)

    # --- Link thư viện cho test ---
    target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
    target_link_libraries(test_costmap PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
    target_link_libraries(test_plugin PRIVATE 
        # ${tf3_LIBRARY}
        costmap_2d
        ${Boost_LIBRARIES}
        Boost::filesystem
        Boost::system
        dl
        pthread
        yaml-cpp
        tf3
        GTest::GTest GTest::Main
    )
endif()