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

# ========================================================
# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
# ========================================================

# Detect if building with Catkin
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building robot_costmap_2d with Catkin")
else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_costmap_2d with Standalone CMake")
endif()

project(robot_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}/robot_costmap_2d")
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_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

if(BUILDING_WITH_CATKIN)
  find_package(catkin REQUIRED COMPONENTS
    robot_std_msgs
    robot_sensor_msgs
    geometry_msgs
    robot_nav_msgs
    robot_map_msgs
    robot_laser_geometry
    robot_visualization_msgs
    robot_voxel_grid
    tf3
    robot_tf3_geometry_msgs
    robot_tf3_sensor_msgs
    data_convert
    robot_xmlrpcpp
    robot_cpp
    robot_time
  )
endif()

# ========================================================
# Catkin specific configuration
# ========================================================

if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES robot_costmap_2d plugins
    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
    DEPENDS Eigen3 PCL
  )
endif()

# --- Define macro để dùng trong code ---
add_definitions(-DROBOT_COSTMAP_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

if(BUILDING_WITH_CATKIN)
  include_directories(${catkin_INCLUDE_DIRS})
endif()

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

# --- Core library: robot_costmap_2d ---
# Tạo thư viện chính
add_library(robot_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 ---
if(BUILDING_WITH_CATKIN)
    target_link_libraries(robot_costmap_2d
        ${catkin_LIBRARIES}
        ${Boost_LIBRARIES}        # Boost
        yaml-cpp
        dl
        ${PCL_LIBRARIES}
    )
else()
  # Standalone mode: link dependencies with PUBLIC visibility to propagate include paths
  target_link_libraries(robot_costmap_2d
    PUBLIC
        robot_std_msgs                   
        robot_sensor_msgs
        geometry_msgs
        robot_nav_msgs
        robot_map_msgs
        robot_laser_geometry
        robot_visualization_msgs
        robot_voxel_grid
        tf3
        robot_tf3_geometry_msgs
        robot_tf3_sensor_msgs
        data_convert
        robot_xmlrpcpp                  # XMLRPC
        robot_cpp 
        PRIVATE
        ${Boost_LIBRARIES}        # Boost
        yaml-cpp
        dl
        ${PCL_LIBRARIES}
  )
endif()

# --- Include directories cho target ---
target_include_directories(robot_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
        )

if(BUILDING_WITH_CATKIN)
  add_dependencies(robot_costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()

# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
install(TARGETS robot_costmap_2d
  EXPORT robot_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 robot_costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/robot_costmap_2d/robot_costmap_2dTargets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(robot_costmap_2d REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
install(EXPORT robot_costmap_2d-targets 
  FILE robot_costmap_2d-targets.cmake
  NAMESPACE robot_costmap_2d::
  DESTINATION lib/cmake/robot_costmap_2d
)

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

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

if(BUILDING_WITH_CATKIN)
  target_link_libraries(plugins 
    PRIVATE
        robot_costmap_2d 
        ${catkin_LIBRARIES}
        ${Boost_LIBRARIES} 
        yaml-cpp
  )
else()
  # In standalone mode, robot_costmap_2d already has PUBLIC links to all dependencies
  # so we just need to link it with PUBLIC visibility to get the include paths
  target_link_libraries(plugins 
    PUBLIC
        robot_costmap_2d 
    PRIVATE
        ${Boost_LIBRARIES} 
        yaml-cpp
        robot_time
        robot_cpp 
  )
endif()

set_target_properties(plugins PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)

if(BUILDING_WITH_CATKIN)
  add_dependencies(plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()

install(TARGETS plugins
  EXPORT plugins-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
)

install(EXPORT plugins-targets 
  FILE plugins-targets.cmake
  NAMESPACE robot_costmap_2d::
  DESTINATION lib/cmake/plugins
)


# --- Option để bật/tắt test ---
option(BUILD_COSTMAP_TESTS "Build robot_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 robot_costmap_2d GTest::GTest GTest::Main pthread)
    target_link_libraries(test_costmap PRIVATE robot_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
        robot_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/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
    set_target_properties(test_costmap PROPERTIES
        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
    set_target_properties(test_plugin PROPERTIES
        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
        LINK_FLAGS "-Wl,--disable-new-dtags"
    )
endif()