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

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

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

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
add_compile_options(-Wall -Wextra -Wpedantic)

# ========================================================
# Dependencies common
# ========================================================
find_package(Boost REQUIRED COMPONENTS thread)
find_package(Eigen3 REQUIRED CONFIG)
if(NOT EIGEN3_INCLUDE_DIRS AND Eigen3_INCLUDE_DIRS)
  set(EIGEN3_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
endif()
if(NOT EIGEN3_LIBRARIES AND Eigen3_LIBRARIES)
  set(EIGEN3_LIBRARIES ${Eigen3_LIBRARIES})
endif()

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

  find_package(catkin REQUIRED COMPONENTS
    robot_angles
    robot_costmap_2d
    robot_std_msgs
    robot_geometry_msgs
    robot_sensor_msgs
    robot_nav_msgs
    robot_cpp
    tf3
    robot_tf3_geometry_msgs
    voxel_grid
    data_convert
  )


  # generate_messages(
  #   DEPENDENCIES std_msgs
  # )

endif()

# ========================================================
# Dynamic reconfigure (only for Catkin)
# ========================================================
if(BUILDING_WITH_CATKIN)

  # set(CATKIN_ENV_HOOK_CMAKE_PATH_SETUP_CUSTOM_PYTHONPATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake/setup_custom_pythonpath.sh.in)

  # generate_dynamic_reconfigure_options(
  #   cfg/BaseLocalPlanner.cfg
  # )

  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES robot_base_local_planner
    CATKIN_DEPENDS
      robot_angles
      robot_costmap_2d
      robot_std_msgs
      robot_geometry_msgs
      robot_sensor_msgs
      robot_nav_msgs
      robot_cpp
      tf3
      robot_tf3_geometry_msgs
      robot_voxel_grid
      data_convert
    DEPENDS Boost
  )

  include_directories(
    include
    ${catkin_INCLUDE_DIRS}
    ${EIGEN3_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
  )

else()

  # =========================
  # Standalone build
  # =========================
  set(CMAKE_POSITION_INDEPENDENT_CODE ON)

  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")

endif()

# ========================================================
# Check headers
# ========================================================
include(CheckIncludeFile)
check_include_file(sys/time.h HAVE_SYS_TIME_H)
if(HAVE_SYS_TIME_H)
  add_definitions(-DHAVE_SYS_TIME_H)
endif()

# ========================================================
# Libraries
# ========================================================
add_library(robot_base_local_planner SHARED
  src/footprint_helper.cpp
  src/goal_functions.cpp
  src/map_cell.cpp
  src/map_grid.cpp
  src/map_grid_visualizer.cpp
  src/map_grid_cost_function.cpp
  src/obstacle_cost_function.cpp
  src/oscillation_cost_function.cpp
  src/prefer_forward_cost_function.cpp
  src/point_grid.cpp
  src/costmap_model.cpp
  src/simple_scored_sampling_planner.cpp
  src/simple_trajectory_generator.cpp
  src/trajectory.cpp
  src/twirling_cost_function.cpp
  src/voxel_grid_model.cpp
)

if(BUILDING_WITH_CATKIN)
  add_dependencies(robot_base_local_planner
    # ${PROJECT_NAME}_gencfg
    # ${PROJECT_NAME}_generate_messages_cpp
    # nav_msgs_generate_messages_cpp
    ${catkin_EXPORTED_TARGETS}
  )
endif()

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

target_link_libraries(robot_base_local_planner
  PUBLIC
    ${Boost_LIBRARIES}
    Eigen3::Eigen
)

if(BUILDING_WITH_CATKIN)
  target_link_libraries(robot_base_local_planner PUBLIC ${catkin_LIBRARIES})
endif()


# ========================================================
# Install
# ========================================================
if(BUILDING_WITH_CATKIN)

  install(TARGETS
    robot_base_local_planner
    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}
  )

else()

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

  install(TARGETS
    robot_base_local_planner
    EXPORT ${PROJECT_NAME}-targets
    LIBRARY DESTINATION lib
    ARCHIVE DESTINATION lib
  )

  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
  )

  install(FILES blp_plugin.xml
    DESTINATION share/${PROJECT_NAME}
  )

  message(STATUS "=================================")
  message(STATUS "Project: ${PROJECT_NAME}")
  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
  message(STATUS "Libraries: robot_base_local_planner")
  message(STATUS "=================================")

endif()

# ========================================================
# Testing
# ========================================================
# if(BUILDING_WITH_CATKIN AND CATKIN_ENABLE_TESTING)
#   find_package(rostest REQUIRED)

#   catkin_add_gtest(base_local_planner_utest
#     test/gtest_main.cpp
#     test/utest.cpp
#     test/velocity_iterator_test.cpp
#     test/footprint_helper_test.cpp
#     test/trajectory_generator_test.cpp
#     test/map_grid_test.cpp
#   )

#   target_link_libraries(base_local_planner_utest
#     robot_base_local_planner
#   )

#   catkin_add_gtest(line_iterator
#     test/line_iterator_test.cpp
#   )
# endif()