cmake_minimum_required(VERSION 3.14.4)
project(swri_transform_util)

set(CMAKE_CXX_STANDARD 17)

find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(geographic_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(gps_msgs REQUIRED)
find_package(marti_nav_msgs REQUIRED)
find_package(OpenCV REQUIRED calib3d core imgproc video)
find_package(rcl_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(swri_math_util REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)
find_package(yaml-cpp REQUIRED)

# check for modern yaml-cpp target - if not found,
# fall back to YAML_CPP_LIBRARIES
if(TARGET yaml-cpp::yaml-cpp)
  set(YAML_CPP_TARGET "yaml-cpp::yaml-cpp")
else()
  set(YAML_CPP_TARGET ${YAML_CPP_LIBRARIES})
endif()

# tf2_geometry_msgs 0.12.1 broke API compatibility, so check which version
# we need to use
if("${tf2_geometry_msgs_VERSION}" VERSION_GREATER "0.12.0")
  set(USE_NEW_TF2_TOMSG 1)
else()
  set(USE_NEW_TF2_TOMSG 0)
endif()

# tf2_ros 0.46.1 removed the deprecated TransformListener/TransformBroadcaster
# constructor overloads that accepted a Node::SharedPtr, requiring a bare node
# reference instead. Older tf2_ros releases (e.g. on Kilted) don't provide
# tf2_ros/version.h at all, so this has to be a CMake-time check rather than a
# runtime TF2_ROS_VERSION_GTE macro.
if("${tf2_ros_VERSION}" VERSION_GREATER_EQUAL "0.46.1")
  set(USE_NEW_TF2_ROS_CTORS 1)
else()
  set(USE_NEW_TF2_ROS_CTORS 0)
endif()

find_package(PkgConfig REQUIRED)
pkg_check_modules(PROJ proj)
if(NOT "${PROJ_FOUND}")
  set(PROJ_INCLUDE_DIRS /usr/include)
  set(PROJ_LIBRARY_DIRS /usr/lib)
  find_library(PROJ_LIBRARIES proj
    HINTS ${PROJ_LIB_DIRS}
  )
endif()

# Geographiclib installs FindGeographicLib.cmake to this non-standard location.
# This is needed only to build swri_transform_util itself; GeographicLib is
# linked PRIVATE and hidden from the exported interface, so downstream packages
# no longer need this module-path workaround.
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/")
find_package(GeographicLib REQUIRED)

add_library(${PROJECT_NAME} SHARED
  src/georeference.cpp
  src/local_xy_util.cpp
  src/utm_util.cpp
  src/transform.cpp
  src/transformer.cpp
  src/transform_manager.cpp
  src/transform_util.cpp
  src/tf2_util.cpp
  src/utm_transformer.cpp
  src/wgs84_transformer.cpp)
target_include_directories(${PROJECT_NAME}
  PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
  INTERFACE
    ${PROJ_INCLUDE_DIRS}
)

target_compile_definitions(${PROJECT_NAME}
  PUBLIC
    USE_NEW_TF2_TOMSG=${USE_NEW_TF2_TOMSG}
    USE_NEW_TF2_ROS_CTORS=${USE_NEW_TF2_ROS_CTORS})

target_link_libraries(${PROJECT_NAME} PUBLIC
  opencv_calib3d
  opencv_core
  opencv_imgproc
  opencv_video
  ${PROJ_LIBRARIES}
  ${geometry_msgs_TARGETS}
  rclcpp::rclcpp
  swri_math_util::swri_math_util
  tf2::tf2
  tf2_geometry_msgs::tf2_geometry_msgs
  tf2_ros::tf2_ros
  ${YAML_CPP_TARGET})

# GeographicLib is an implementation detail (hidden behind a forward declaration
# in local_xy_util.h), so link it PRIVATE. Downstream consumers no longer need
# GeographicLib's headers or its non-standard CMake config location.
target_link_libraries(${PROJECT_NAME} PRIVATE
  ${GeographicLib_LIBRARIES})

add_library(${PROJECT_NAME}_nodes SHARED
  src/nodes/dynamic_transform_publisher.cpp
  src/nodes/gps_transform_publisher.cpp
  src/nodes/obstacle_transformer.cpp)

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

target_include_directories(${PROJECT_NAME}_nodes
  SYSTEM
  INTERFACE
    ${PROJ_INCLUDE_DIRS})

target_compile_definitions(${PROJECT_NAME}_nodes
  PRIVATE "COMPOSITION_BUILDING_DLL")

rclcpp_components_register_nodes(${PROJECT_NAME}_nodes "swri_transform_util::DynamicTransformPublisher")
rclcpp_components_register_nodes(${PROJECT_NAME}_nodes "swri_transform_util::GpsTransformPublisher")
rclcpp_components_register_nodes(${PROJECT_NAME}_nodes "swri_transform_util::ObstacleTransformer")
target_link_libraries(${PROJECT_NAME}_nodes PRIVATE
  ${PROJECT_NAME}
  ${geographic_msgs_TARGETS}
  ${gps_msgs_TARGETS}
  ${marti_nav_msgs_TARGETS}
  ${rcl_interfaces_TARGETS}
  ${rclcpp_components_TARGETS}
)

add_executable(lat_lon_tf_echo src/nodes/lat_lon_tf_echo.cpp)
target_include_directories(lat_lon_tf_echo PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)

target_include_directories(lat_lon_tf_echo
  SYSTEM
  INTERFACE
  ${PROJ_INCLUDE_DIRS})

target_link_libraries(lat_lon_tf_echo PRIVATE
  ${PROJECT_NAME}
  ${geographic_msgs_TARGETS}
  ${gps_msgs_TARGETS})

if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  find_package(ament_cmake_pytest REQUIRED)
  find_package(ament_index_cpp REQUIRED)
  find_package(launch_testing_ament_cmake)

  ament_add_gtest_executable(transform_manager_test test/test_transform_manager.cpp)
  target_include_directories(transform_manager_test PRIVATE
    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)

  target_link_libraries(transform_manager_test
    ${PROJECT_NAME}
    rclcpp::rclcpp
    tf2::tf2
    tf2_ros::tf2_ros)

  ament_add_gtest(transform_test test/test_transform.cpp)
  target_link_libraries(transform_test
    ${PROJECT_NAME}
    rclcpp::rclcpp
    tf2::tf2)

  ament_add_gtest(local_xy_util_test test/test_local_xy_util.cpp)
  target_link_libraries(local_xy_util_test
    ${PROJECT_NAME}
    rclcpp::rclcpp)

  ament_add_pytest_test(origin_manager_test test/test_origin_manager.py)

  ament_add_gtest(utm_util_test test/test_utm_util.cpp)
  target_link_libraries(utm_util_test
    ${PROJECT_NAME}
    rclcpp::rclcpp)

  ament_add_gtest(georeference_test test/test_georeference.cpp)
  target_link_libraries(georeference_test
    ${PROJECT_NAME}
    ament_index_cpp::ament_index_cpp
    rclcpp::rclcpp
    tf2::tf2)
  # Note: which ament_index_cpp API the test uses is selected in the source with
  # AMENT_INDEX_CPP_VERSION_GTE from <ament_index_cpp/version.h>.

  ament_add_gtest(transform_util_test test/test_transform_util.cpp)
  target_link_libraries(transform_util_test
    ${PROJECT_NAME}
    rclcpp::rclcpp
    tf2::tf2)

  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/launch/transform_manager.test.py)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/launch/local_xy_util.test.py)
  # The parametrized launch tests run one launch per case, so they are given
  # more than the 60 second default that add_launch_test() applies per test.
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/lat_lon_tf_echo.test.py TIMEOUT 180)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/lat_lon_tf_echo_unsupported_origin.test.py)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_invalid_gps.test.py)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_invalid_navsat.test.py)
  # The "custom" functionality appears to be gone in ROS 2.
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_auto_gps.test.py TIMEOUT 180)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_auto_navsat.test.py)
  add_launch_test(${CMAKE_CURRENT_SOURCE_DIR}/test/initialize_origin_manual.test.py TIMEOUT 180)

  install(TARGETS
    transform_manager_test
    utm_util_test
    georeference_test
    transform_util_test
    local_xy_util_test
    DESTINATION lib/${PROJECT_NAME}
  )

  install(DIRECTORY test
    DESTINATION share/${PROJECT_NAME}
  )

endif()

install(DIRECTORY include/
  DESTINATION include/${PROJECT_NAME}
)

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}/
)

install(TARGETS
  ${PROJECT_NAME}
  EXPORT export_${PROJECT_NAME}
  RUNTIME DESTINATION bin
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib)

install(TARGETS
  ${PROJECT_NAME}_nodes
  lat_lon_tf_echo
  RUNTIME DESTINATION bin
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib)

### Install Python Nodes/Scripts ###
install(PROGRAMS nodes/initialize_origin.py
  DESTINATION lib/${PROJECT_NAME})

ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_python_install_package(${PROJECT_NAME})
ament_export_dependencies(
  ament_cmake
  geometry_msgs
  OpenCV
  rclcpp
  swri_math_util
  tf2
  tf2_geometry_msgs
  tf2_ros
  yaml_cpp_vendor
  yaml-cpp)
ament_package()

