cmake_minimum_required(VERSION 3.5)
project(odin_ros_driver)

option(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION "Enable YOLO+ByteTrack target observation in odin_ros_driver" ON)


if(DEFINED BUILD_SYSTEM)
    set(ROS_VERSION ${BUILD_SYSTEM})
    message(STATUS "ROS_VERSION: ${ROS_VERSION}")
elseif(DEFINED ENV{ROS_DISTRO})
    if("$ENV{ROS_DISTRO}" MATCHES "foxy|galactic|humble|iron|rolling")
        set(ROS_VERSION "ROS2")
    else()
        set(ROS_VERSION "ROS1")
    endif()
elseif(DEFINED ENV{ROS_VERSION})
    if("$ENV{ROS_VERSION}" EQUAL "2")
        set(ROS_VERSION "ROS2")
    else()
        set(ROS_VERSION "ROS1")
    endif()
else()
    # Attempt automatic detection
    if(COMMAND catkin_package)
        set(ROS_VERSION "ROS1")
    elseif(COMMAND ament_package)
        set(ROS_VERSION "ROS2")
    else()
        # Default to ROS2
        set(ROS_VERSION "ROS2")
        message(WARNING "Unable to determine ROS version, defaulting to ROS2")
    endif()
endif()

# Add compile definitions after detecting ROS version
if(ROS_VERSION STREQUAL "ROS2")
    add_definitions(-DROS2)
    message(STATUS "Defining ROS2")
else()
    add_definitions(-DROS1)
    message(STATUS "Defining ROS1")
endif()

message(STATUS "Build system: ${ROS_VERSION}")

# Platform detection
execute_process(
    COMMAND uname -m
    OUTPUT_VARIABLE ARCH
    OUTPUT_STRIP_TRAILING_WHITESPACE
)

if(ARCH STREQUAL "x86_64")
    set(TARGET_PLATFORM "x86")
    message(STATUS "Detected x86_64 architecture")
elseif(ARCH MATCHES "arm|aarch64")
    set(TARGET_PLATFORM "arm")
    message(STATUS "Detected ARM architecture: ${ARCH}")
else()
    message(WARNING "Unsupported architecture: ${ARCH}. Using default settings")
    set(TARGET_PLATFORM "unknown")
endif()

# Set library path
set(LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib")
message(STATUS "Library directory: ${LIB_DIR}")

# Set library name based on platform
if(TARGET_PLATFORM STREQUAL "arm")
    set(LYD_HOST_API_LIB_NAME "lydHostApi_arm")
else()
    set(LYD_HOST_API_LIB_NAME "lydHostApi_amd")
endif()

# Find precompiled lydHostApi library
find_library(LYD_HOST_API_LIB
    NAMES 
        ${LYD_HOST_API_LIB_NAME}
        lib${LYD_HOST_API_LIB_NAME}.a
        lib${LYD_HOST_API_LIB_NAME}.so
    PATHS ${LIB_DIR}
    NO_DEFAULT_PATH
)

if(LYD_HOST_API_LIB)
    message(STATUS "Found lydHostApi library: ${LYD_HOST_API_LIB}")
else()
    file(GLOB LIB_FILES "${LIB_DIR}/lib${LYD_HOST_API_LIB_NAME}.*")
    if(LIB_FILES)
        message(STATUS "Found library files: ${LIB_FILES}")
        set(LYD_HOST_API_LIB ${LIB_FILES})
    else()
        message(FATAL_ERROR "Could not find precompiled lydHostApi library in ${LIB_DIR}")
    endif()
endif()

# Set common compile options
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

# Set optimization flags
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")

set(TARGET_PREDICTION_ROOT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../../..")
set(TARGET_PREDICTION_DEPLOY_DIR "${TARGET_PREDICTION_ROOT_DIR}/deploy")
set(TARGET_PREDICTION_THIRDPARTY_DIR "${TARGET_PREDICTION_ROOT_DIR}/thirdparty")
set(ODIN_TARGET_OBSERVATION_AVAILABLE OFF)
set(ODIN_TARGET_OBSERVATION_READY OFF)

if(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION AND
   EXISTS "${TARGET_PREDICTION_THIRDPARTY_DIR}/YOLOs-CPP-TensorRT/include/yolos/tasks/pose.hpp" AND
   EXISTS "${TARGET_PREDICTION_THIRDPARTY_DIR}/motcpp/CMakeLists.txt")
    enable_language(CUDA)
    find_package(CUDAToolkit REQUIRED)

    find_path(TENSORRT_INCLUDE_DIR NvInfer.h
        HINTS
            ${TENSORRT_DIR}/include
            $ENV{TENSORRT_DIR}/include
            /home/hjy/Library/TensorRT-10.3.0.26/include
            /usr/include
            /usr/include/x86_64-linux-gnu
            /usr/include/aarch64-linux-gnu
            /usr/local/include
            /usr/local/TensorRT/include
    )
    find_library(NVINFER_LIB nvinfer
        HINTS
            ${TENSORRT_DIR}/lib
            $ENV{TENSORRT_DIR}/lib
            /home/hjy/Library/TensorRT-10.3.0.26/lib
            /usr/lib
            /usr/lib/x86_64-linux-gnu
            /usr/lib/aarch64-linux-gnu
            /usr/local/lib
            /usr/local/TensorRT/lib
    )

    if(TENSORRT_INCLUDE_DIR AND NVINFER_LIB)
        set(MOTCPP_BUILD_TESTS OFF CACHE BOOL "" FORCE)
        set(MOTCPP_BUILD_BENCHMARKS OFF CACHE BOOL "" FORCE)
        set(MOTCPP_BUILD_EXAMPLES OFF CACHE BOOL "" FORCE)
        set(MOTCPP_BUILD_TOOLS OFF CACHE BOOL "" FORCE)
        set(MOTCPP_ENABLE_ONNX OFF CACHE BOOL "" FORCE)
        set(MOTCPP_INSTALL OFF CACHE BOOL "" FORCE)
        set(CMAKE_POSITION_INDEPENDENT_CODE ON)
        add_subdirectory(
            ${TARGET_PREDICTION_THIRDPARTY_DIR}/motcpp
            ${CMAKE_CURRENT_BINARY_DIR}/motcpp
            EXCLUDE_FROM_ALL
        )
        set(ODIN_TARGET_OBSERVATION_READY ON)
    else()
        message(WARNING "TensorRT not found. Target observation support disabled.")
    endif()
elseif(ODIN_DRIVER_ENABLE_TARGET_OBSERVATION)
    message(WARNING "TargetPrediction thirdparty target-observation dependencies not found. Target observation support disabled.")
endif()

# Find common dependencies
find_package(PkgConfig REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(OpenSSL REQUIRED)
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)

# Shared include directories
include_directories(
    include
    ${EIGEN3_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
    ${yaml-cpp_INCLUDE_DIR}
    ${LIBUSB_INCLUDE_DIRS}
    ${CMAKE_CURRENT_SOURCE_DIR}/include
)

# Shared library list
set(COMMON_LIBS
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
    ${yaml-cpp_LIBRARIES}
    ${OPENSSL_LIBRARIES}
    ${LIBUSB_LIBRARIES}
    pthread
    rt
    ${CMAKE_DL_LIBS}
    ${LYD_HOST_API_LIB}
)

if(ODIN_TARGET_OBSERVATION_READY)
    add_library(target_observation_processing STATIC
        src/target_observation_processing.cpp
        src/reid_trt_extractor.cpp
        ${TARGET_PREDICTION_THIRDPARTY_DIR}/YOLOs-CPP-TensorRT/include/yolos/core/cuda_preprocessing.cu
    )
    target_include_directories(target_observation_processing PUBLIC
        include
        ${TARGET_PREDICTION_THIRDPARTY_DIR}/YOLOs-CPP-TensorRT/include
        ${TENSORRT_INCLUDE_DIR}
    )
    target_link_libraries(target_observation_processing
        Eigen3::Eigen
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        motcpp::motcpp
        ${NVINFER_LIB}
        CUDA::cudart
    )
    target_compile_definitions(target_observation_processing PUBLIC ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
    set_target_properties(target_observation_processing PROPERTIES
        CUDA_SEPARABLE_COMPILATION ON
        POSITION_INDEPENDENT_CODE ON
    )
    set(ODIN_TARGET_OBSERVATION_AVAILABLE ON)
    message(STATUS "Target observation support enabled in odin_ros_driver")
endif()

function(set_runtime_search_path target_name runtime_path)
    if(TARGET ${target_name})
        set_target_properties(${target_name} PROPERTIES
            BUILD_RPATH "${runtime_path}"
            INSTALL_RPATH "${runtime_path}"
        )
    endif()
endfunction()

# ===== ROS1 Configuration =====
if(ROS_VERSION STREQUAL "ROS1")
    message(STATUS "Configuring for ROS1 build")
    
    find_package(catkin REQUIRED COMPONENTS
        roscpp
        std_msgs
        sensor_msgs
        nav_msgs
        cv_bridge
        tf
        image_transport
    )
    
    include_directories(${catkin_INCLUDE_DIRS})
    
    catkin_package(
        CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport
        INCLUDE_DIRS include
    )
    
    # Set output directories
    set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME})
    set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
    set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
    
    add_executable(host_sdk_sample 
        src/host_sdk_sample.cpp
        src/yaml_parser.cpp
        src/rawCloudRender.cpp
        src/camera_pose_visualization.cpp
    )
    target_link_libraries(host_sdk_sample
        ${catkin_LIBRARIES}
        ${COMMON_LIBS}
	${LYD_HOST_API_LIB}
    	${LIBUSB_LIBRARIES}
    	yaml-cpp
    	${OpenCV_LIBS}
    	pthread  
        usb-1.0
    )

    add_library(pointcloud_depth_converter src/pointcloud_depth_converter.cpp)
    target_link_libraries(pointcloud_depth_converter
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
    )
    add_library(depth_image_ros_node src/depth_image_ros_node.cpp)
    target_link_libraries(depth_image_ros_node
    pointcloud_depth_converter
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
    )
    add_executable(pcd2depth_node src/pcd2depth_ros.cpp)
    target_link_libraries(pcd2depth_node
    depth_image_ros_node
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
    )

    add_library(cloud_reprojector src/cloud_reprojector.cpp)
    target_link_libraries(cloud_reprojector
    ${OpenCV_LIBS}
    ${PCL_LIBRARIES}
    )

    add_executable(cloud_reprojection_node
        src/cloud_reprojection_ros.cpp
        src/cloud_reprojection_processing.cpp
    )
    target_link_libraries(cloud_reprojection_node
        cloud_reprojector
        ${catkin_LIBRARIES}
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
    )
    if(ODIN_TARGET_OBSERVATION_AVAILABLE)
        target_link_libraries(cloud_reprojection_node target_observation_processing)
        target_compile_definitions(cloud_reprojection_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
    endif()

    add_executable(image_overlay_node src/image_overlay_node.cpp)
    target_link_libraries(image_overlay_node
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
    )

    # Installation rules
    install(TARGETS host_sdk_sample
        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
    )
    
    install(DIRECTORY include/
        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
        FILES_MATCHING PATTERN "*.h"
    )
    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
        install(DIRECTORY config/
            DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
        )
    endif()

# ===== ROS2 Configuration =====
elseif(ROS_VERSION STREQUAL "ROS2")
    message(STATUS "Configuring for ROS2 build")
    set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries for ROS2 targets" FORCE)
    
    # Find all necessary ROS2 packages
    find_package(ament_cmake REQUIRED)
    find_package(rosidl_default_generators REQUIRED)
    find_package(rclcpp REQUIRED)
    find_package(std_msgs REQUIRED)
    find_package(sensor_msgs REQUIRED)
    find_package(nav_msgs REQUIRED)
    find_package(visualization_msgs REQUIRED)
    find_package(cv_bridge REQUIRED)
    find_package(image_transport REQUIRED)
    find_package(pcl_conversions REQUIRED)
    find_package(message_filters REQUIRED)
    find_package(tf2 REQUIRED)
    find_package(tf2_ros REQUIRED)
    find_package(geometry_msgs REQUIRED)
    find_package(tf2_geometry_msgs REQUIRED)

    rosidl_generate_interfaces(${PROJECT_NAME}
        "msg/TargetObservation.msg"
        "msg/TargetObservationArray.msg"
        DEPENDENCIES std_msgs geometry_msgs nav_msgs
    )

    rosidl_get_typesupport_target(odin_ros_driver_interfaces_target
        ${PROJECT_NAME} "rosidl_typesupport_cpp")
    rosidl_get_typesupport_target(odin_ros_driver_fastrtps_cpp_target
        ${PROJECT_NAME} "rosidl_typesupport_fastrtps_cpp")
    
    # Create executable
    add_executable(host_sdk_sample 
        src/host_sdk_sample.cpp
        src/yaml_parser.cpp  
        src/rawCloudRender.cpp
        src/camera_pose_visualization.cpp
        )
    
    # Link libraries
    target_link_libraries(host_sdk_sample
        ${COMMON_LIBS}
        yaml-cpp
        usb-1.0
    )
    
    # Add ROS2 dependencies
    ament_target_dependencies(host_sdk_sample
        rclcpp 
        std_msgs 
        sensor_msgs 
        nav_msgs 
        visualization_msgs
        cv_bridge
        image_transport
        tf2
        tf2_ros
        geometry_msgs
        tf2_geometry_msgs
    )
    
    add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
    target_link_libraries(pointcloud_depth_converter_ros2
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
    )
    
    add_library(depth_image_ros2_node_lib src/depth_image_ros2_node.cpp)
    target_link_libraries(depth_image_ros2_node_lib
        pointcloud_depth_converter_ros2
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
    )
    
    ament_target_dependencies(depth_image_ros2_node_lib
        rclcpp
        sensor_msgs
        std_msgs
        cv_bridge
        image_transport
        pcl_conversions
        message_filters
    )

    add_executable(pcd2depth_ros2_node src/pcd2depth_ros2.cpp)
    target_link_libraries(pcd2depth_ros2_node
        depth_image_ros2_node_lib
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        yaml-cpp
    )
    
    ament_target_dependencies(pcd2depth_ros2_node
        rclcpp
        sensor_msgs
        std_msgs
        cv_bridge
        image_transport
        pcl_conversions
        message_filters
    )

    add_library(cloud_reprojector_ros2 src/cloud_reprojector.cpp)
    target_compile_definitions(cloud_reprojector_ros2 PRIVATE ROS2)
    target_link_libraries(cloud_reprojector_ros2
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
    )

    add_executable(cloud_reprojection_ros2_node
        src/cloud_reprojection_ros.cpp
        src/cloud_reprojection_processing.cpp
    )
    target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
    target_link_options(cloud_reprojection_ros2_node PRIVATE "-Wl,--no-as-needed")
    target_link_libraries(cloud_reprojection_ros2_node
        cloud_reprojector_ros2
        ${odin_ros_driver_interfaces_target}
        ${odin_ros_driver_fastrtps_cpp_target}
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        yaml-cpp
    )
    if(ODIN_TARGET_OBSERVATION_AVAILABLE)
        target_link_libraries(cloud_reprojection_ros2_node target_observation_processing)
        target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
    endif()
    ament_target_dependencies(cloud_reprojection_ros2_node
        rclcpp
        sensor_msgs
        nav_msgs
        geometry_msgs
        std_msgs
        visualization_msgs
        cv_bridge
        pcl_conversions
        message_filters
    )

    add_executable(cloud_reprojection_synced_ros2_node
        src/cloud_reprojection_synced_ros.cpp
        src/cloud_reprojection_processing.cpp
    )
    target_compile_definitions(cloud_reprojection_synced_ros2_node PRIVATE ROS2)
    target_link_options(cloud_reprojection_synced_ros2_node PRIVATE "-Wl,--no-as-needed")
    target_link_libraries(cloud_reprojection_synced_ros2_node
        cloud_reprojector_ros2
        ${odin_ros_driver_interfaces_target}
        ${odin_ros_driver_fastrtps_cpp_target}
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        yaml-cpp
    )
    if(ODIN_TARGET_OBSERVATION_AVAILABLE)
        target_link_libraries(cloud_reprojection_synced_ros2_node target_observation_processing)
        target_compile_definitions(cloud_reprojection_synced_ros2_node PRIVATE ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION)
    endif()
    ament_target_dependencies(cloud_reprojection_synced_ros2_node
        rclcpp
        sensor_msgs
        nav_msgs
        geometry_msgs
        std_msgs
        visualization_msgs
        cv_bridge
        pcl_conversions
    )

    add_executable(image_overlay_node src/image_overlay_node.cpp)
    target_compile_definitions(image_overlay_node PRIVATE ROS2)
    target_link_libraries(image_overlay_node
        ${OpenCV_LIBS}
    )
    ament_target_dependencies(image_overlay_node
        rclcpp
        sensor_msgs
        cv_bridge
        image_transport
        message_filters
    )

    foreach(ros2_runtime_target
        host_sdk_sample
        pcd2depth_ros2_node
        cloud_reprojection_ros2_node
        cloud_reprojection_synced_ros2_node
        image_overlay_node
    )
        set_runtime_search_path(${ros2_runtime_target} "\$ORIGIN/..")
    endforeach()

    foreach(ros2_library_target
        pointcloud_depth_converter_ros2
        depth_image_ros2_node_lib
        cloud_reprojector_ros2
    )
        set_runtime_search_path(${ros2_library_target} "\$ORIGIN")
    endforeach()

    set_runtime_search_path(motcpp "\$ORIGIN")

    # Installation rules - ensure all install targets are defined before ament_package()
    # Install executable
    install(TARGETS 
        pointcloud_depth_converter_ros2
        depth_image_ros2_node_lib
        cloud_reprojector_ros2
        host_sdk_sample 
        pcd2depth_ros2_node
        cloud_reprojection_ros2_node
        cloud_reprojection_synced_ros2_node
        image_overlay_node
        EXPORT export_${PROJECT_NAME}
        ARCHIVE DESTINATION lib
        LIBRARY DESTINATION lib
        RUNTIME DESTINATION lib/${PROJECT_NAME}
    )
    if(TARGET motcpp)
        install(TARGETS motcpp
            LIBRARY DESTINATION lib
            ARCHIVE DESTINATION lib
        )
    endif()
    
    # Install package.xml
    install(FILES package.xml
        DESTINATION share/${PROJECT_NAME}
    )
    
    # Install headers
    install(DIRECTORY include/
        DESTINATION include
    )
    # Install launch_ROS2 directory
    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch_ROS2")
        install(DIRECTORY launch_ROS2/
            DESTINATION share/${PROJECT_NAME}/launch
        )
        message(STATUS "Installing launch_ROS2 directory to share/${PROJECT_NAME}/launch")
    else()
        message(WARNING "launch_ROS2 directory not found")
    endif()
    # Install config files
    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
        install(DIRECTORY config/
            DESTINATION share/${PROJECT_NAME}/config
        )
    endif()
    
    # Install launch files
    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
        install(DIRECTORY launch/
            DESTINATION share/${PROJECT_NAME}/launch
        )
    endif()
    ament_export_targets(export_${PROJECT_NAME})
    # Declare dependencies
    ament_export_dependencies(
        rclcpp 
        rosidl_default_runtime
        std_msgs 
        sensor_msgs 
        nav_msgs 
        visualization_msgs
        cv_bridge
        image_transport
        pcl_conversions
        message_filters
        tf2
        tf2_ros
        geometry_msgs
        tf2_geometry_msgs
    )
    
    ament_package()
    message(STATUS "Install targets added")
    
else()
    message(FATAL_ERROR "Invalid ROS_VERSION: ${ROS_VERSION}")
endif()

# ARM platform specific link options
if(TARGET_PLATFORM STREQUAL "arm")
    set_target_properties(host_sdk_sample PROPERTIES
        LINK_FLAGS "-Wl,--no-as-needed -Wl,--rpath=${LIB_DIR}"
    )
    message(STATUS "Adding ARM-specific link options and RPATH")
endif()

# Add debug information
message(STATUS "=======================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
message(STATUS "Target platform: ${TARGET_PLATFORM}")
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
message(STATUS "=======================================")