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()
# if (NOT TARGET laser_geometry)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../laser_geometry ${CMAKE_BINARY_DIR}/laser_geometry_build)
# endif()
# if (NOT TARGET voxel_grid)
#     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
# endif()

include_directories(
    include  
    ${EIGEN3_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
)
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
    laser_geometry
    voxel_grid
    # ${TF2_LIBRARY}
    tf3
)
target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})

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

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

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

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

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

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

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

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

add_library(costmap_2d_robot SHARED
    src/costmap_2d_robot.cpp
)
target_link_libraries(costmap_2d_robot
    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
    static_layer
    obstacle_layer
    inflation_layer
    ${Boost_LIBRARIES}
    Boost::filesystem
    Boost::system
    dl
    pthread
    yaml-cpp
)