cmake_minimum_required(VERSION 3.20)
project(fastsim_se2_planner VERSION 0.1.2 LANGUAGES CXX)

include(GNUInstallDirs)
include(CMakePackageConfigHelpers)

option(FASTSIM_SE2_BUILD_PYTHON "Build the pybind11 extension" ON)
option(FASTSIM_SE2_BUILD_TESTS "Build standalone C++ tests" ON)
option(FASTSIM_SE2_BUILD_BENCHMARKS "Build benchmark executable" ON)
option(FASTSIM_SE2_BUILD_CLI "Build command-line planner" ON)
option(FASTSIM_SE2_BUILD_EXAMPLES "Build standalone examples" ON)
option(FASTSIM_SE2_ENABLE_ASAN "Enable AddressSanitizer" OFF)
option(FASTSIM_SE2_ENABLE_UBSAN "Enable UndefinedBehaviorSanitizer" OFF)
set(FASTSIM_OMPL_ROOT "" CACHE PATH "Optional user-space OMPL prefix")

if(MSVC)
  add_compile_options(/W4)
  if(FASTSIM_SE2_ENABLE_ASAN OR FASTSIM_SE2_ENABLE_UBSAN)
    message(FATAL_ERROR "FASTSIM SE(2) sanitizer options require GCC or Clang")
  endif()
elseif(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
  set(FASTSIM_SE2_SANITIZERS "")
  if(FASTSIM_SE2_ENABLE_ASAN)
    list(APPEND FASTSIM_SE2_SANITIZERS address)
  endif()
  if(FASTSIM_SE2_ENABLE_UBSAN)
    list(APPEND FASTSIM_SE2_SANITIZERS undefined)
  endif()
  if(FASTSIM_SE2_SANITIZERS)
    list(JOIN FASTSIM_SE2_SANITIZERS "," FASTSIM_SE2_SANITIZER_LIST)
    add_compile_options(-fsanitize=${FASTSIM_SE2_SANITIZER_LIST} -fno-omit-frame-pointer)
    add_link_options(-fsanitize=${FASTSIM_SE2_SANITIZER_LIST})
  endif()
elseif(FASTSIM_SE2_ENABLE_ASAN OR FASTSIM_SE2_ENABLE_UBSAN)
  message(FATAL_ERROR "FASTSIM SE(2) sanitizer options require GCC or Clang")
endif()

find_path(OMPL_INCLUDE_DIR ompl/base/spaces/DubinsStateSpace.h
  HINTS ${FASTSIM_OMPL_ROOT}/usr/include/ompl-1.5
  PATH_SUFFIXES ompl-1.5)
find_library(OMPL_LIBRARY ompl HINTS ${FASTSIM_OMPL_ROOT}/usr/lib/x86_64-linux-gnu)
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
if(NOT OMPL_INCLUDE_DIR OR NOT OMPL_LIBRARY)
  message(FATAL_ERROR "Native OMPL is required; set FASTSIM_OMPL_ROOT to its prefix")
endif()
if(NOT TARGET OMPL::OMPL)
  add_library(OMPL::OMPL UNKNOWN IMPORTED)
  set_target_properties(OMPL::OMPL PROPERTIES
    IMPORTED_LOCATION "${OMPL_LIBRARY}"
    INTERFACE_INCLUDE_DIRECTORIES "${OMPL_INCLUDE_DIR}"
  )
endif()

add_library(fastsim_se2_planner
  src/costmap.cpp
  src/collision_checker.cpp
  src/footprint.cpp
  src/io.cpp
  src/hybrid_astar.cpp
  src/kinematic_heuristic.cpp
  src/motion_primitives.cpp
  src/obstacle_heuristic.cpp
  src/path_smoother.cpp
  src/planner_config.cpp
  src/visualization.cpp
)
add_library(fastsim::se2_planner ALIAS fastsim_se2_planner)
target_compile_features(fastsim_se2_planner PUBLIC cxx_std_17)
target_include_directories(fastsim_se2_planner PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_link_libraries(fastsim_se2_planner PUBLIC OMPL::OMPL)
target_link_libraries(fastsim_se2_planner PRIVATE Eigen3::Eigen)
set_target_properties(fastsim_se2_planner PROPERTIES
  VERSION ${PROJECT_VERSION}
  SOVERSION ${PROJECT_VERSION_MAJOR}
  POSITION_INDEPENDENT_CODE ON
  CXX_EXTENSIONS OFF
)

get_filename_component(FASTSIM_OMPL_LIBRARY_DIR "${OMPL_LIBRARY}" DIRECTORY)
function(fastsim_configure_runtime target)
  set_target_properties(${target} PROPERTIES
    BUILD_RPATH "${FASTSIM_OMPL_LIBRARY_DIR}"
    INSTALL_RPATH_USE_LINK_PATH ON
  )
  if(UNIX AND NOT APPLE)
    target_link_options(${target} PRIVATE "-Wl,--disable-new-dtags")
  endif()
endfunction()

if(FASTSIM_SE2_BUILD_CLI)
  add_executable(fastsim-se2-plan tools/plan_cli.cpp)
  target_link_libraries(fastsim-se2-plan PRIVATE fastsim_se2_planner)
  fastsim_configure_runtime(fastsim-se2-plan)
endif()

if(FASTSIM_SE2_BUILD_EXAMPLES)
  add_executable(se2_planner_example examples/standalone.cpp)
  target_link_libraries(se2_planner_example PRIVATE fastsim_se2_planner)
  fastsim_configure_runtime(se2_planner_example)
endif()

if(FASTSIM_SE2_BUILD_BENCHMARKS)
  add_executable(se2_planner_bench benchmarks/benchmark.cpp)
  target_link_libraries(se2_planner_bench PRIVATE fastsim_se2_planner)
  fastsim_configure_runtime(se2_planner_bench)
endif()

install(TARGETS fastsim_se2_planner
  EXPORT fastsim_se2_plannerTargets
  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
)
if(TARGET fastsim-se2-plan)
  if(SKBUILD)
    set_target_properties(fastsim-se2-plan PROPERTIES
      OUTPUT_NAME "_fastsim-se2-plan")
    install(TARGETS fastsim-se2-plan
      RUNTIME DESTINATION fastsim_navigation
      COMPONENT python
    )
  else()
    install(TARGETS fastsim-se2-plan
      RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
      COMPONENT python
    )
  endif()
endif()
install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
install(EXPORT fastsim_se2_plannerTargets
  FILE fastsim_se2_plannerTargets.cmake
  NAMESPACE fastsim::
  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/fastsim_se2_planner
)
configure_package_config_file(
  cmake/fastsim_se2_plannerConfig.cmake.in
  ${CMAKE_CURRENT_BINARY_DIR}/fastsim_se2_plannerConfig.cmake
  INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/fastsim_se2_planner
)
write_basic_package_version_file(
  ${CMAKE_CURRENT_BINARY_DIR}/fastsim_se2_plannerConfigVersion.cmake
  VERSION ${PROJECT_VERSION}
  COMPATIBILITY SameMajorVersion
)
install(FILES
  ${CMAKE_CURRENT_BINARY_DIR}/fastsim_se2_plannerConfig.cmake
  ${CMAKE_CURRENT_BINARY_DIR}/fastsim_se2_plannerConfigVersion.cmake
  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/fastsim_se2_planner
)

if(FASTSIM_SE2_BUILD_PYTHON)
  find_package(pybind11 CONFIG REQUIRED)
  pybind11_add_module(_se2_planner bindings/python_module.cpp)
  target_link_libraries(_se2_planner PRIVATE fastsim_se2_planner)
  target_compile_features(_se2_planner PRIVATE cxx_std_17)
  fastsim_configure_runtime(_se2_planner)
  set_target_properties(_se2_planner PROPERTIES
    INSTALL_RPATH "$ORIGIN/.libs"
  )
  if(SKBUILD)
    install(TARGETS _se2_planner
      LIBRARY DESTINATION fastsim_navigation
      COMPONENT python
    )
  else()
    install(TARGETS _se2_planner LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
  endif()
endif()

if(FASTSIM_SE2_BUILD_TESTS)
  enable_testing()
  add_executable(se2_planner_tests tests/test_core.cpp)
  target_link_libraries(se2_planner_tests PRIVATE fastsim_se2_planner)
  fastsim_configure_runtime(se2_planner_tests)
  add_test(NAME se2_planner_core COMMAND se2_planner_tests)
endif()
