cmake_minimum_required(VERSION 3.16)
project(mujoco_ros2_control_plugins)

find_package(ros2_control_cmake REQUIRED)
set_compiler_options()
export_windows_symbols()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(backward_ros REQUIRED)
find_package(rclcpp REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(pluginlib REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(mujoco_ros2_control_msgs REQUIRED)
find_package(glfw3 REQUIRED)
find_package(OpenGL REQUIRED COMPONENTS EGL)

# Attempt to link MuJoCo via the vendor package, if available
find_package(mujoco_vendor QUIET)
if(mujoco_vendor_FOUND)
  set(MUJOCO_LIB mujoco::mujoco)
  set(MUJOCO_INCLUDE_DIR ${mujoco_vendor_INCLUDE_DIRS})
else()
  # Fallback to system MuJoCo or other methods if needed
  message(FATAL_ERROR "mujoco_vendor not found!")
endif()

# The base is just a header, so we can setup an interface library for external consumers.
add_library(mujoco_ros2_control_plugins INTERFACE)
target_include_directories(mujoco_ros2_control_plugins INTERFACE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
target_include_directories(mujoco_ros2_control_plugins SYSTEM INTERFACE
  $<BUILD_INTERFACE:${MUJOCO_INCLUDE_DIR}>
)
target_link_libraries(mujoco_ros2_control_plugins INTERFACE
  ${MUJOCO_LIB}
  rclcpp::rclcpp
)

# Build the base plugin library (all plugins compiled into one shared library)
add_library(mujoco_ros2_control_plugins_impl SHARED
  src/heartbeat_publisher_plugin.cpp
  src/external_wrench_plugin.cpp
  src/base_velocity_plugin.cpp
  src/free_joint_state_publisher_plugin.cpp
  src/fts_grav_comp_plugin.cpp
  src/camera_plugin.cpp
  src/mujoco_3d_lidar_plugin.cpp
  # TODO: Remove once this has been fully deprecated
  src/rangefinder_lidar_plugin.cpp
)
target_link_libraries(mujoco_ros2_control_plugins_impl PUBLIC
  mujoco_ros2_control_plugins
  pluginlib::pluginlib
  realtime_tools::realtime_tools
  ${geometry_msgs_TARGETS}
  ${sensor_msgs_TARGETS}
  ${std_msgs_TARGETS}
  ${std_srvs_TARGETS}
  ${visualization_msgs_TARGETS}
  ${mujoco_ros2_control_msgs_TARGETS}
  glfw
  OpenGL::EGL
  OpenGL::OpenGL
)
target_compile_definitions(mujoco_ros2_control_plugins_impl PRIVATE
  REALTIME_TOOLS_VERSION_MAJOR=${realtime_tools_VERSION_MAJOR}
)

# Install the interface for external use
install(
  TARGETS mujoco_ros2_control_plugins
  EXPORT export_${PROJECT_NAME}
)

# And we don't care about exporting the impl
install(
  TARGETS mujoco_ros2_control_plugins_impl
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
)

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

  ament_add_gtest(test_external_wrench_plugin
    test/test_external_wrench_plugin.cpp
  )
  target_include_directories(test_external_wrench_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_external_wrench_plugin
    mujoco_ros2_control_plugins_impl
    ${mujoco_ros2_control_msgs_TARGETS}
    ${visualization_msgs_TARGETS}
  )

  # MuJoCo has TinyXML2 statically linked, which conflicts with the system TinyXML2
  # used by FastDDS. We need to ensure the system TinyXML2 is loaded first.
  # Force tinyxml2 to appear first in the DT_NEEDED entries by using LINK_OPTIONS.
  # TODO: Remove this after the following is resolved and the next ROS sync
  # https://github.com/pal-robotics/mujoco_vendor/issues/2
  target_link_options(test_external_wrench_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )

  ament_add_gtest(test_free_joint_state_publisher_plugin
    test/test_free_joint_state_publisher_plugin.cpp
  )
  target_include_directories(test_free_joint_state_publisher_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_free_joint_state_publisher_plugin
    mujoco_ros2_control_plugins_impl
    ${mujoco_ros2_control_msgs_TARGETS}
  )
  target_link_options(test_free_joint_state_publisher_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )

  ament_add_gtest(test_camera_plugin
    test/test_camera_plugin.cpp
  )
  target_include_directories(test_camera_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_camera_plugin
    mujoco_ros2_control_plugins_impl
    ${sensor_msgs_TARGETS}
    ${std_srvs_TARGETS}
  )
  target_link_options(test_camera_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )

  ament_add_gtest(test_3d_lidar_plugin
    test/test_3d_lidar_plugin.cpp
  )
  target_include_directories(test_3d_lidar_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_3d_lidar_plugin
    mujoco_ros2_control_plugins_impl
    ${sensor_msgs_TARGETS}
    ament_index_cpp::ament_index_cpp
  )
  target_link_options(test_3d_lidar_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )

  ament_add_gtest(test_base_velocity_plugin
    test/test_base_velocity_plugin.cpp
  )
  target_include_directories(test_base_velocity_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_base_velocity_plugin
    mujoco_ros2_control_plugins_impl
    ${geometry_msgs_TARGETS}
  )
  target_link_options(test_base_velocity_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )

  ament_add_gtest(test_fts_grav_comp_plugin
    test/test_fts_grav_comp_plugin.cpp
  )
  target_include_directories(test_fts_grav_comp_plugin PRIVATE
    ${CMAKE_CURRENT_SOURCE_DIR}/src
  )
  target_link_libraries(test_fts_grav_comp_plugin
    mujoco_ros2_control_plugins_impl
    ${geometry_msgs_TARGETS}
  )
  target_link_options(test_fts_grav_comp_plugin PUBLIC
    "LINKER:--push-state,--no-as-needed"
    "$<TARGET_LINKER_FILE:tinyxml2::tinyxml2>"
    "LINKER:--pop-state"
  )
endif()

# Install include directory
install(DIRECTORY include/
  DESTINATION include
)

# Export plugin description
pluginlib_export_plugin_description_file(mujoco_ros2_control_plugins mujoco_ros2_control_plugins.xml)

ament_export_include_directories(include)
ament_export_dependencies(mujoco_vendor rclcpp pluginlib ros2_control_cmake)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)

ament_package()
