# cmake_minimum_required(VERSION 3.10)
# project(costmap_2d)

# set(CMAKE_CXX_STANDARD 17)
# set(CMAKE_POSITION_INDEPENDENT_CODE ON)

# find_package(tf2 REQUIRED)

# if (NOT TARGET sensor_msgs)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
# endif()

# if (NOT TARGET geometry_msgs)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
# endif()

# if (NOT TARGET nav_msgs)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build)
# endif()

# if (NOT TARGET map_msgs)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
# endif()


# # ---- Dependencies ----
# find_package(Eigen3 REQUIRED)
# find_package(Boost REQUIRED COMPONENTS system thread)
# find_package(GTest REQUIRED)
# find_package(PCL REQUIRED COMPONENTS common io)

# include_directories(
#     include  
#     ${EIGEN3_INCLUDE_DIRS}
#     ${Boost_INCLUDE_DIRS}
#     ${GTEST_INCLUDE_DIRS}
#     ${PCL_INCLUDE_DIRS}
#     /usr/include
# )
# link_directories(${PCL_LIBRARY_DIRS})

# add_definitions(${EIGEN3_DEFINITIONS}
# ${PCL_DEFINITIONS})

# # ---- Core costmap_2d library ----
# add_library(costmap_2d 
#     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
# )

# target_link_libraries(costmap_2d
#     ${Boost_LIBRARIES}
#     std_msgs
#     sensor_msgs
#     geometry_msgs
#     nav_msgs
#     map_msgs
#     # tf2
# )
# target_include_directories(costmap_2d PRIVATE ${Boost_INCLUDE_DIRS})

# # # ---- Layer plugins ----
# add_library(layers SHARED
#     # plugins/inflation_layer.cpp
#     # plugins/obstacle_layer.cpp
#     plugins/static_layer.cpp
#     # plugins/voxel_layer.cpp
#     # plugins/preferred_layer.cpp
#     # plugins/unpreferred_layer.cpp
#     # plugins/critical_layer.cpp
#     # plugins/directional_layer.cpp
# )

# target_link_libraries(layers
#     costmap_2d
#     ${Boost_LIBRARIES}
#     yaml-cpp
# )



# # ---- Example Executable ----
# 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)
# target_link_libraries(test_array_parser PRIVATE costmap_2d
#                     PRIVATE GTest::GTest
#                     PRIVATE GTest::Main
#                     PRIVATE pthread)
# target_link_libraries(test_costmap PRIVATE costmap_2d
#                     PRIVATE GTest::GTest
#                     PRIVATE GTest::Main
#                     PRIVATE pthread)
# target_link_libraries(test_plugin PRIVATE
#                     # /usr/lib/libtf2.so 
#                     tf2
#                     costmap_2d
#                     ${Boost_LIBRARIES}
#                     Boost::filesystem
#                     Boost::system
#                     dl
#                     pthread
#                     yaml-cpp)

# set_target_properties(test_plugin PROPERTIES
#   BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d"
#   INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d"
# )

cmake_minimum_required(VERSION 3.10)
project(costmap_2d)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)

# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
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 ---
# find_package(tf2 REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread filesystem)
find_package(GTest REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)
set(TF2_LIBRARY /usr/lib/libtf2.so)

# --- Include other message packages if needed ---
if (NOT TARGET sensor_msgs)
    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
endif()
if (NOT TARGET geometry_msgs)
    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
endif()
if (NOT TARGET nav_msgs)
    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build)
endif()
if (NOT TARGET map_msgs)
    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
endif()

include_directories(
    include  
    ${EIGEN3_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
    /usr/include
)
link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})

# --- Core library ---
add_library(costmap_2d_new 
    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
)

target_link_libraries(costmap_2d_new
    ${Boost_LIBRARIES}
    std_msgs
    sensor_msgs
    geometry_msgs
    nav_msgs
    map_msgs
    ${TF2_LIBRARY}
)
target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})

# --- Plugin libraries ---
add_library(layers SHARED
    plugins/static_layer.cpp
)
target_link_libraries(layers
    costmap_2d_new
    ${Boost_LIBRARIES}
    yaml-cpp
)

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

target_link_libraries(test_array_parser PRIVATE
    costmap_2d_new
    GTest::GTest
    GTest::Main
    pthread
)

target_link_libraries(test_costmap PRIVATE
    costmap_2d_new
    GTest::GTest
    GTest::Main
    pthread
)

target_link_libraries(test_plugin PRIVATE
    ${TF2_LIBRARY}
    costmap_2d_new
    layers
    ${Boost_LIBRARIES}
    Boost::filesystem
    Boost::system
    dl
    pthread
    yaml-cpp
)
