cmake_minimum_required(VERSION 3.8)
project(unity_moveit2_bridge)

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

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

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_move_group REQUIRED)
find_package(moveit_kinematics REQUIRED)
# find_package(moveit_planners_ompl REQUIRED)  # Temporarily disabled - path issue in Jazzy
find_package(moveit_servo REQUIRED)
find_package(robot_state_publisher REQUIRED)
# joint_state_publisher is a Python package, no CMake config needed
# find_package(joint_state_publisher REQUIRED)
find_package(urdf REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(shape_msgs REQUIRED)
find_package(octomap_msgs REQUIRED)
find_package(rosbridge_library REQUIRED)

# Include directories
include_directories(
  include
  ${rclcpp_INCLUDE_DIRS}
  ${moveit_core_INCLUDE_DIRS}
  ${moveit_ros_planning_interface_INCLUDE_DIRS}
)

# C++ executables
set(DEPENDENCIES
  rclcpp
  std_msgs
  geometry_msgs
  sensor_msgs
  trajectory_msgs
  control_msgs
  moveit_msgs
  tf2
  tf2_ros
  tf2_geometry_msgs
  moveit_core
  moveit_ros_planning
  moveit_ros_planning_interface
  moveit_ros_move_group
  moveit_kinematics
  # moveit_planners_ompl
  moveit_servo
  robot_state_publisher
  # joint_state_publisher is Python-only, no C++ dependency needed
  urdf
  geometric_shapes
  shape_msgs
  octomap_msgs
)

# Temporarily compile only compatible services
# Unity Bridge Server and Trajectory Service have API issues with ROS2 Jazzy
# Robot State Publisher has been completely refactored in Jazzy

# MoveIt2 Planning Service - Only include existing source files
add_executable(moveit2_planning_service
  src/moveit2_planning_service.cpp
)
ament_target_dependencies(moveit2_planning_service ${DEPENDENCIES})

# Collision Detection Service - Only include existing source files
add_executable(collision_detection_service
  src/collision_detection_service.cpp
)
ament_target_dependencies(collision_detection_service ${DEPENDENCIES})

# Install C++ executables
install(TARGETS
  moveit2_planning_service
  collision_detection_service
  DESTINATION lib/${PROJECT_NAME}
)

# TODO: Update these services for ROS2 Jazzy API compatibility
# - unity_bridge_server: ExecuteKnownTrajectory service not available
# - trajectory_execution_service: execution_result message not available
# - robot_state_publisher_node: Complete API refactor in Jazzy

# Install Python modules
ament_python_install_package(${PROJECT_NAME})

# Install Python scripts - Only install existing scripts
install(PROGRAMS
  scripts/robot_controller.py
  scripts/unity_bridge_client.py
  DESTINATION lib/${PROJECT_NAME}
)

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

# Install config files
install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}/
)

# Install URDF and mesh files - Only if they exist
# install(DIRECTORY
#   urdf
#   meshes
#   DESTINATION share/${PROJECT_NAME}/
# )

# Install RViz config - Only if it exists
# install(DIRECTORY
#   rviz
#   DESTINATION share/${PROJECT_NAME}/
# )

# Install header files - Only if they exist
# install(DIRECTORY
#   include/
#   DESTINATION include/
# )

# Testing - Temporarily disabled until test files are created
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # Linting only
  ament_lint_auto_find_test_dependencies()

  # TODO: Add tests when test files are created
  # ament_add_gtest(test_robot_interface test/test_robot_interface.cpp)
  # ament_add_pytest_test(test_unity_bridge test/test_unity_bridge.py)
endif()

# Export dependencies
ament_export_dependencies(${DEPENDENCIES})
ament_export_include_directories(include)

ament_package()