# --- 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

# --- 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                   
    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 vào hệ thống khi chạy make install ---
install(TARGETS costmap_2d
  EXPORT costmap_2d-targets
  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
)

# --- Xuất export set costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/costmap_2d/costmap_2dTargets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(costmap_2d REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE costmap_2d::costmap_2d) ---
install(EXPORT costmap_2d-targets 
  FILE costmap_2d-targets.cmake
  NAMESPACE costmap_2d::
  DESTINATION lib/cmake/costmap_2d
)

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

# --- Plugin libraries ---
# Tạo các plugin shared library
add_library(plugins 
    SHARED 
        plugins/static_layer.cpp
        plugins/obstacle_layer.cpp
        plugins/inflation_layer.cpp
        plugins/voxel_layer.cpp
        plugins/critical_layer.cpp
        plugins/directional_layer.cpp
        plugins/preferred_layer.cpp
        plugins/unpreferred_layer.cpp
        )

target_link_libraries(plugins 
        costmap_2d 
        ${Boost_LIBRARIES} 
        yaml-cpp
        robot_time
        )


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

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 
    
        ${Boost_LIBRARIES}
        Boost::filesystem
        Boost::system
        dl
        pthread
        yaml-cpp
        tf3
        robot_time
        costmap_2d
        GTest::GTest GTest::Main
    )
    
    # --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
    set_target_properties(test_array_parser PROPERTIES
        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
    set_target_properties(test_costmap PROPERTIES
        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
    set_target_properties(test_plugin PROPERTIES
        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
endif()