cmake_minimum_required(VERSION 3.0.2)
project(recovery_core VERSION 0.1.0 LANGUAGES CXX)

# ========================================================
# Build mode detection
# ========================================================
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building recovery_core with Catkin")
else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building recovery_core with Standalone CMake")
endif()


# ========================================================
# C++ Standard
# ========================================================
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)


# ========================================================
# Common dependencies
# ========================================================
find_package(Boost REQUIRED COMPONENTS
  system
  thread
  filesystem
)

find_package(Threads REQUIRED)
find_package(yaml-cpp REQUIRED)


# ========================================================
# Standalone configuration
# ========================================================
if(NOT BUILDING_WITH_CATKIN)

  # Enable Position Independent Code
  set(CMAKE_POSITION_INDEPENDENT_CODE ON)

  # Runtime search path configuration
  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")


  # ------------------------------------------------------
  # Navigation source tree
  # ------------------------------------------------------
  set(PNKX_NAV_CORE_SRC_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../.."
  )

  set(WORKSPACE_DEVEL_LIB_DIR
    "${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../../devel/lib"
  )


  # ------------------------------------------------------
  # Collect standalone package include directories
  # ------------------------------------------------------
  file(GLOB STANDALONE_PACKAGE_INCLUDE_DIRS
    LIST_DIRECTORIES true

    ${PNKX_NAV_CORE_SRC_DIR}/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/include
    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/*/include
  )


  # robot_costmap_2d headers may expose PCL dependencies
  find_package(PCL QUIET COMPONENTS
    common
    io
  )


  set(STANDALONE_INCLUDE_DIRS
    ${STANDALONE_PACKAGE_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}

    /opt/ros/noetic/include
    /usr/local/include
  )


  if(PCL_FOUND)
    add_definitions(${PCL_DEFINITIONS})
  endif()


  # ------------------------------------------------------
  # Standalone libraries
  # ------------------------------------------------------
  set(PACKAGES_DIR
    robot_costmap_2d
    robot_cpp
    robot_time
    robot_xmlrpcpp
  )


  # ------------------------------------------------------
  # TF3
  # ------------------------------------------------------
  find_library(TF3_LIBRARY
    NAMES tf3

    PATHS
      /usr/lib
      /usr/local/lib
      /usr/lib/x86_64-linux-gnu
  )


  # ------------------------------------------------------
  # Library search paths
  # ------------------------------------------------------
  if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
    link_directories(${WORKSPACE_DEVEL_LIB_DIR})
  endif()

  link_directories(
    /usr/local/lib
  )


# ========================================================
# Catkin configuration
# ========================================================
else()

  find_package(catkin REQUIRED COMPONENTS
    robot_costmap_2d
    robot_cpp
    robot_time
    robot_geometry_msgs
    robot_nav_msgs
    robot_xmlrpcpp
  )


  find_library(TF3_LIBRARY
    NAMES tf3

    PATHS
      /usr/lib
      /usr/local/lib
      /usr/lib/x86_64-linux-gnu
  )


  catkin_package(
    INCLUDE_DIRS
      include

    LIBRARIES
      recovery_core
      recovery_core_clear_costmap_recovery
      recovery_core_rotate_recovery
      recovery_core_back_up_recovery
      recovery_core_regen_path_recovery

    CATKIN_DEPENDS
      robot_costmap_2d
      robot_cpp
      robot_time
      robot_geometry_msgs
      robot_nav_msgs
      robot_xmlrpcpp

    DEPENDS
      Boost
  )


  include_directories(
    include

    ${catkin_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${TF3_INCLUDE_DIR}
  )

endif()


# ========================================================
# Core library
# ========================================================
add_library(recovery_core SHARED
  src/recovery_types.cpp
  src/recovery_config.cpp
  src/recovery_behavior.cpp
)


# ========================================================
# Core library - Catkin
# ========================================================
if(BUILDING_WITH_CATKIN)

  add_dependencies(
    recovery_core

    ${${PROJECT_NAME}_EXPORTED_TARGETS}
    ${catkin_EXPORTED_TARGETS}
  )


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

      ${Boost_INCLUDE_DIRS}
      ${TF3_INCLUDE_DIR}
  )


  target_link_libraries(recovery_core
    PUBLIC
      ${catkin_LIBRARIES}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::thread
      Boost::filesystem

      yaml-cpp

      Threads::Threads

      ${CMAKE_DL_LIBS}

      ${TF3_LIBRARY}
  )


# ========================================================
# Core library - Standalone
# ========================================================
else()

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

    PRIVATE
      ${STANDALONE_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${TF3_INCLUDE_DIR}
  )


  target_link_libraries(recovery_core
    PUBLIC
      ${PACKAGES_DIR}

    PRIVATE
      Boost::boost
      Boost::system
      Boost::thread
      Boost::filesystem

      yaml-cpp

      Threads::Threads

      ${CMAKE_DL_LIBS}

      ${TF3_LIBRARY}
  )


  set_target_properties(recovery_core PROPERTIES
    LIBRARY_OUTPUT_DIRECTORY
      ${CMAKE_BINARY_DIR}

    BUILD_RPATH
      "${CMAKE_BINARY_DIR}"

    INSTALL_RPATH
      "${CMAKE_INSTALL_PREFIX}/lib"
  )

endif()


# ========================================================
# Recovery plugin helper
# ========================================================
set(RECOVERY_CORE_PLUGIN_TARGETS)


function(add_recovery_core_plugin target source)

  add_library(${target} SHARED
    ${source}
  )


  add_dependencies(
    ${target}
    recovery_core
  )


  # ======================================================
  # Catkin plugin
  # ======================================================
  if(BUILDING_WITH_CATKIN)

    add_dependencies(
      ${target}

      ${${PROJECT_NAME}_EXPORTED_TARGETS}
      ${catkin_EXPORTED_TARGETS}
    )


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


    target_link_libraries(${target}
      PUBLIC
        recovery_core

      PRIVATE
        ${catkin_LIBRARIES}

        Boost::boost
        Boost::system
        Boost::thread
        Boost::filesystem

        yaml-cpp

        Threads::Threads

        ${CMAKE_DL_LIBS}

        ${TF3_LIBRARY}
    )


  # ======================================================
  # Standalone plugin
  # ======================================================
  else()

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

      PRIVATE
        ${STANDALONE_INCLUDE_DIRS}
        ${TF3_INCLUDE_DIR}
    )


    target_link_libraries(${target}
      PUBLIC
        recovery_core

      PRIVATE
        Boost::boost
        Boost::system
        Boost::thread
        Boost::filesystem

        yaml-cpp

        Threads::Threads

        ${CMAKE_DL_LIBS}

        ${TF3_LIBRARY}
    )


    set_target_properties(${target} PROPERTIES
      LIBRARY_OUTPUT_DIRECTORY
        ${CMAKE_BINARY_DIR}

      BUILD_RPATH
        "${CMAKE_BINARY_DIR}"

      INSTALL_RPATH
        "${CMAKE_INSTALL_PREFIX}/lib"
    )

  endif()


  set_target_properties(${target} PROPERTIES
    POSITION_INDEPENDENT_CODE ON
  )


  list(APPEND
    RECOVERY_CORE_PLUGIN_TARGETS
    ${target}
  )


  set(
    RECOVERY_CORE_PLUGIN_TARGETS
    ${RECOVERY_CORE_PLUGIN_TARGETS}
    PARENT_SCOPE
  )

endfunction()


# ========================================================
# Recovery plugins
# ========================================================
add_recovery_core_plugin(
  recovery_core_clear_costmap_recovery
  plugins/clear_costmap_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_rotate_recovery
  plugins/rotate_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_back_up_recovery
  plugins/back_up_recovery.cpp
)

add_recovery_core_plugin(
  recovery_core_regen_path_recovery
  plugins/regen_path_recovery.cpp
)


# ========================================================
# Install - Catkin
# ========================================================
if(BUILDING_WITH_CATKIN)

  install(
    TARGETS
      recovery_core
      ${RECOVERY_CORE_PLUGIN_TARGETS}

    ARCHIVE DESTINATION
      ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION
      ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION
      ${CATKIN_GLOBAL_BIN_DESTINATION}
  )


  install(
    DIRECTORY
      include/${PROJECT_NAME}/

    DESTINATION
      ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

    FILES_MATCHING
      PATTERN "*.h"
      PATTERN "*.hpp"

    PATTERN ".svn" EXCLUDE
  )


# ========================================================
# Install - Standalone
# ========================================================
else()

  install(
    TARGETS
      recovery_core
      ${RECOVERY_CORE_PLUGIN_TARGETS}

    EXPORT
      ${PROJECT_NAME}-targets

    ARCHIVE DESTINATION
      lib

    LIBRARY DESTINATION
      lib

    RUNTIME DESTINATION
      bin
  )


  install(
    EXPORT
      ${PROJECT_NAME}-targets

    FILE
      ${PROJECT_NAME}-targets.cmake

    NAMESPACE
      ${PROJECT_NAME}::

    DESTINATION
      lib/cmake/${PROJECT_NAME}
  )


  install(
    DIRECTORY
      include/${PROJECT_NAME}/

    DESTINATION
      include

    FILES_MATCHING
      PATTERN "*.h"
      PATTERN "*.hpp"

    PATTERN ".svn" EXCLUDE
  )


  # ------------------------------------------------------
  # Print configuration information
  # ------------------------------------------------------
  message(STATUS "=================================")
  message(STATUS "Project: ${PROJECT_NAME}")
  message(STATUS "Version: ${PROJECT_VERSION}")
  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
  message(STATUS "Libraries:")
  message(STATUS "  recovery_core")
  message(STATUS "  recovery_core_clear_costmap_recovery")
  message(STATUS "  recovery_core_rotate_recovery")
  message(STATUS "  recovery_core_back_up_recovery")
  message(STATUS "  recovery_core_regen_path_recovery")
  message(STATUS "Dependencies:")
  message(STATUS "  robot_costmap_2d")
  message(STATUS "  robot_cpp")
  message(STATUS "  robot_time")
  message(STATUS "  robot_geometry_msgs")
  message(STATUS "  robot_nav_msgs")
  message(STATUS "  robot_xmlrpcpp")
  message(STATUS "  tf3")
  message(STATUS "  Boost")
  message(STATUS "  yaml-cpp")
  message(STATUS "=================================")

endif()


# ========================================================
# Tests
# ========================================================
option(
  BUILD_RECOVERY_CORE_TESTS
  "Build recovery_core tests"
  ON
)


if(BUILD_RECOVERY_CORE_TESTS)

  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/CMakeLists.txt)
    add_subdirectory(test)
  endif()

endif()