cmake_minimum_required(VERSION 3.0.2)
project(robot_costmap_2d VERSION 1.0.0 LANGUAGES CXX)

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

# C++ Standard - must be set before find_package
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

# Find dependencies
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread filesystem)
find_package(GTest REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io)
find_package(yaml-cpp REQUIRED)

if (NOT BUILDING_WITH_CATKIN)

  # Enable Position Independent Code
  set(CMAKE_POSITION_INDEPENDENT_CODE ON)

  # Cấu hình RPATH để tránh cycle trong runtime search path
  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")

  set(PACKAGES_DIR  
    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
  )

else()

# ========================================================
# Catkin specific configuration
# ========================================================
  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
  )

  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  PCL Boost 
  )

  include_directories(
    include
    ${catkin_INCLUDE_DIRS}
    ${EIGEN3_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${GTEST_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
  )
  link_directories(${PCL_LIBRARY_DIRS})
endif()

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

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

# ========================================================
# Core library: robot_costmap_2d
# ========================================================
add_library(robot_costmap_2d SHARED
  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
)

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

  target_include_directories(robot_costmap_2d
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
  )

  target_link_libraries(robot_costmap_2d
    PUBLIC ${catkin_LIBRARIES}
    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
    PRIVATE yaml-cpp
    PRIVATE dl
    PRIVATE ${PCL_LIBRARIES}
  )

else()

  target_include_directories(robot_costmap_2d
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>
      ${EIGEN3_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${PCL_INCLUDE_DIRS}
  )

  target_link_libraries(robot_costmap_2d
    PUBLIC
      ${PACKAGES_DIR}
    PRIVATE
      Boost::boost Boost::system Boost::thread Boost::filesystem
      yaml-cpp
      dl
      ${PCL_LIBRARIES}
  )

  set_target_properties(robot_costmap_2d PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
    BUILD_RPATH "${CMAKE_BINARY_DIR}"
    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
  )

endif()

# ========================================================
# Plugin library: plugins
# ========================================================
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)
  add_dependencies(plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

  target_include_directories(plugins
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>
  )

  target_link_libraries(plugins
    PUBLIC robot_costmap_2d
    PRIVATE ${catkin_LIBRARIES}
    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
    PRIVATE yaml-cpp
  )

else()

  target_include_directories(plugins
    PUBLIC
      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
      $<INSTALL_INTERFACE:include>
  )

  target_link_libraries(plugins
    PUBLIC robot_costmap_2d
    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
    PRIVATE yaml-cpp
    PRIVATE robot_time
    PRIVATE robot_cpp
  )

  set_target_properties(plugins PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
    BUILD_RPATH "${CMAKE_BINARY_DIR}"
    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
  )

endif()

# ========================================================
# Install
# ========================================================
if(BUILDING_WITH_CATKIN)
  ## Mark libraries for installation
  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
  install(TARGETS robot_costmap_2d plugins
    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
  )

  ## Mark cpp header files for installation
  install(DIRECTORY include/${PROJECT_NAME}/
    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
    FILES_MATCHING PATTERN "*.h"
    PATTERN ".svn" EXCLUDE
  )

else()

  install(TARGETS robot_costmap_2d plugins
    EXPORT ${PROJECT_NAME}-targets
    ARCHIVE DESTINATION lib
    LIBRARY DESTINATION lib
    RUNTIME DESTINATION bin
  )

  # Export targets
  install(EXPORT ${PROJECT_NAME}-targets
    FILE ${PROJECT_NAME}-targets.cmake
    NAMESPACE ${PROJECT_NAME}::
    DESTINATION lib/cmake/${PROJECT_NAME}
  )

  ## Mark cpp header files for installation
  install(DIRECTORY include/${PROJECT_NAME}/
    DESTINATION include
    FILES_MATCHING PATTERN "*.h"
    PATTERN ".svn" EXCLUDE
  )

  # Print configuration info
  message(STATUS "=================================")
  message(STATUS "Project: ${PROJECT_NAME}")
  message(STATUS "Version: ${PROJECT_VERSION}")
  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
  message(STATUS "Libraries: robot_costmap_2d, plugins")
  message(STATUS "Dependencies: 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, Eigen3, PCL, Boost, yaml-cpp")
  message(STATUS "=================================")
endif()

# ========================================================
# Test executables
# ========================================================
option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)

if(BUILD_COSTMAP_TESTS)
  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
    add_executable(test_array_parser test/array_parser_test.cpp)
    target_link_libraries(test_array_parser PRIVATE
      robot_costmap_2d
      GTest::GTest
      GTest::Main
      Threads::Threads
    )
  endif()

  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
    add_executable(test_costmap test/coordinates_test.cpp)
    target_link_libraries(test_costmap PRIVATE
      robot_costmap_2d
      GTest::GTest
      GTest::Main
      Threads::Threads
    )
  endif()

  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
    add_executable(test_plugin test/static_layer_test.cpp)
    target_link_libraries(test_plugin PRIVATE
      robot_costmap_2d
      Boost::boost Boost::filesystem Boost::system
      yaml-cpp
      dl
      Threads::Threads
      tf3
      robot_time
      GTest::GTest
      GTest::Main
    )
  endif()
endif()