cmake_minimum_required(VERSION 3.22)
project(roboplan VERSION 0.4.0)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find dependencies.
find_package(Eigen3 REQUIRED)
find_package(pinocchio REQUIRED)
find_package(hpp-fcl CONFIG QUIET)
if(NOT TARGET hpp-fcl::hpp-fcl AND DEFINED pinocchio_DIR)
  get_filename_component(ROBOPLAN_PINOCCHIO_CMAKE_DIR "${pinocchio_DIR}" DIRECTORY)
  find_package(hpp-fcl CONFIG REQUIRED
    PATHS "${ROBOPLAN_PINOCCHIO_CMAKE_DIR}/hpp-fcl"
    NO_DEFAULT_PATH
  )
endif()
if(NOT TARGET hpp-fcl::hpp-fcl)
  find_package(hpp-fcl CONFIG QUIET)
endif()
if(NOT TARGET hpp-fcl::hpp-fcl)
  find_path(ROBOPLAN_HPP_FCL_INCLUDE_DIR
    NAMES hpp/fcl/fwd.hh
    HINTS "/opt/ros/$ENV{ROS_DISTRO}/include"
  )
  find_library(ROBOPLAN_HPP_FCL_LIBRARY
    NAMES hpp-fcl
    HINTS
      "/opt/ros/$ENV{ROS_DISTRO}/lib/${CMAKE_LIBRARY_ARCHITECTURE}"
      "/opt/ros/$ENV{ROS_DISTRO}/lib"
  )
  if(ROBOPLAN_HPP_FCL_INCLUDE_DIR AND ROBOPLAN_HPP_FCL_LIBRARY)
    add_library(hpp-fcl::hpp-fcl SHARED IMPORTED GLOBAL)
    set_target_properties(hpp-fcl::hpp-fcl PROPERTIES
      IMPORTED_LOCATION "${ROBOPLAN_HPP_FCL_LIBRARY}"
      INTERFACE_INCLUDE_DIRECTORIES "${ROBOPLAN_HPP_FCL_INCLUDE_DIR}"
    )
  endif()
endif()
if(NOT TARGET hpp-fcl::hpp-fcl)
  message(FATAL_ERROR "Failed to resolve hpp-fcl::hpp-fcl")
endif()
find_package(console_bridge REQUIRED)
find_package(yaml-cpp REQUIRED)

# Ament CMake is not immediately required, but we include it at the top so that
# if it is found we can use it in build testing before requiring a
# find_package(ament_cmake_test REQUIRED). Otherwise this can cause infinite recursion
# issues with symlink installs.
find_package(ament_cmake QUIET)

set(TARGETS_LIST roboplan)

# Add tl_expected from source only if not found.
find_package(tl_expected QUIET)
if (NOT tl_expected_FOUND)
    add_library(tl_expected INTERFACE)
    target_include_directories(
        tl_expected
        INTERFACE
            $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>
    )
    list(APPEND TARGETS_LIST tl_expected)
endif()

# Add tinyxml2 from source only if not found.
find_package(tinyxml2_vendor QUIET)
find_package(TinyXML2 QUIET)
if ( NOT tinyxml2_vendor_FOUND AND NOT TinyXML2_FOUND AND NOT TARGET tinyxml2::tinyxml2 )
    add_library(tinyxml2
        include/tinyxml2/tinyxml2.cpp
        include/tinyxml2/tinyxml2.h)
    add_library(tinyxml2::tinyxml2 ALIAS tinyxml2)
    list(APPEND TARGETS_LIST tinyxml2)
endif()

# Add roboplan libraries.
add_library(roboplan SHARED
    src/core/collision_context.cpp
    src/core/path_utils.cpp
    src/core/scene.cpp
    src/core/scene_utils.cpp
    src/core/types.cpp
)
if(NOT TARGET roboplan::roboplan)
    add_library(roboplan::roboplan ALIAS roboplan)
endif()
target_include_directories(roboplan PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
)
target_link_libraries(roboplan
    PUBLIC
    Eigen3::Eigen
    pinocchio::pinocchio
    tinyxml2::tinyxml2
    console_bridge::console_bridge
)
if (yaml-cpp_VERSION VERSION_LESS "0.8")  # for Ubuntu 22.04
    target_link_libraries(roboplan PRIVATE yaml-cpp)
else()
    target_link_libraries(roboplan PRIVATE yaml-cpp::yaml-cpp)
endif()
set_property(TARGET roboplan PROPERTY CXX_STANDARD 20)
if(SKBUILD OR ROBOPLAN_CMEEL)
  set_target_properties(roboplan PROPERTIES
    INSTALL_RPATH "$ORIGIN"
    INSTALL_RPATH_USE_LINK_PATH FALSE
  )
endif()

add_library(filters SHARED
    src/filters/se3_low_pass_filter.cpp
)
target_include_directories(filters PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
)
target_link_libraries(filters
    PUBLIC
    Eigen3::Eigen
    pinocchio::pinocchio
)
set_property(TARGET filters PROPERTY CXX_STANDARD 20)
if(SKBUILD OR ROBOPLAN_CMEEL)
  set_target_properties(filters PROPERTIES
    INSTALL_RPATH "$ORIGIN"
    INSTALL_RPATH_USE_LINK_PATH FALSE
  )
endif()
list(APPEND TARGETS_LIST filters)

set(ROBOPLAN_TARGETS ${TARGETS_LIST})
install(
  TARGETS
  ${ROBOPLAN_TARGETS}
  EXPORT roboplan-targets
  LIBRARY DESTINATION lib
  INCLUDES DESTINATION include
)

# Install headers
install(DIRECTORY include/ DESTINATION include)

# Generate the package configuration file
configure_file(
    "cmake/roboplanConfig.cmake.in"
    "${CMAKE_CURRENT_BINARY_DIR}/roboplanConfig.cmake"
    @ONLY
)

# Create and install the configuration files
include(CMakePackageConfigHelpers)
configure_package_config_file(
    cmake/roboplanConfig.cmake.in
    ${CMAKE_CURRENT_BINARY_DIR}/roboplanConfig.cmake
    INSTALL_DESTINATION lib/cmake/roboplan
)
write_basic_package_version_file(
    ${CMAKE_CURRENT_BINARY_DIR}/roboplanConfigVersion.cmake
    VERSION ${PROJECT_VERSION}
    COMPATIBILITY AnyNewerVersion
)
install(
    FILES
        "${CMAKE_CURRENT_BINARY_DIR}/roboplanConfig.cmake"
        "${CMAKE_CURRENT_BINARY_DIR}/roboplanConfigVersion.cmake"
    DESTINATION
        "lib/cmake/roboplan"
)

# Install the headers and targets
install(DIRECTORY
            include/roboplan
        DESTINATION lib/cmake/)

install(EXPORT roboplan-targets
        FILE roboplanTargets.cmake
        NAMESPACE roboplan::
        DESTINATION lib/cmake/roboplan)

install(TARGETS roboplan
        EXPORT roboplan
        LIBRARY DESTINATION lib
        INCLUDES DESTINATION include
        )

# Build the Python bindings by default (for the scikit-build wheel, colcon, and
# pixi). Override with -DBUILD_PYTHON_BINDINGS=OFF for a pure C++ build.
option(BUILD_PYTHON_BINDINGS "Build the roboplan Python bindings" ON)
if(BUILD_PYTHON_BINDINGS)
  add_subdirectory(bindings)
endif()

# Add the unit tests, if enabled.
option(BUILD_TESTING "" ON)
if (BUILD_TESTING)
    enable_testing()
    add_subdirectory(test)
endif()

# If ament_cmake is detected, export the package to be visible in ROS 2.
if ( ament_cmake_FOUND )
    message(STATUS "ament_cmake found. Exporting for ROS 2.")
    ament_package()
endif()
