cmake_minimum_required(VERSION 3.16)

project(r1_vision LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

option(BUILD_VISION "Build vision components" ON)
option(BUILD_VOICE "Build R1 voice assistant" ON)

set(
    UNITREE_SDK2_DIR
    "/home/jvizner/unitree_sdk2"
    CACHE PATH
    "Path to unitree_sdk2"
)

# ============================================================
# Unitree SDK2
# ============================================================

if(BUILD_VISION OR BUILD_VOICE)

    if(NOT EXISTS "${UNITREE_SDK2_DIR}/CMakeLists.txt")
        message(
            FATAL_ERROR
            "Invalid UNITREE_SDK2_DIR: ${UNITREE_SDK2_DIR}"
        )
    endif()

    add_subdirectory(
        "${UNITREE_SDK2_DIR}"
        "${CMAKE_BINARY_DIR}/unitree_sdk2"
        EXCLUDE_FROM_ALL
    )

endif()

# ============================================================
# Vision / Motion
# ============================================================

if(BUILD_VISION)

    find_package(Eigen3 REQUIRED)

    find_package(
        OpenCV REQUIRED
        COMPONENTS
            core
            imgproc
            highgui
            imgcodecs
            calib3d
    )

    find_package(pinocchio REQUIRED)
    find_package(fmt REQUIRED)

    # --------------------------------------------------------
    # Shared vision dependencies
    # --------------------------------------------------------

    add_library(
        r1_vision_common
        INTERFACE
    )

    target_include_directories(
        r1_vision_common
        INTERFACE
            "${CMAKE_CURRENT_SOURCE_DIR}/include"
            "${UNITREE_SDK2_DIR}/include"
            ${OpenCV_INCLUDE_DIRS}
    )

    target_link_libraries(
        r1_vision_common
        INTERFACE
            Eigen3::Eigen
            pinocchio::pinocchio
            ${OpenCV_LIBS}
            unitree_sdk2
    )

    target_compile_definitions(
        r1_vision_common
        INTERFACE
            UNITREE_SDK2_DIR="${UNITREE_SDK2_DIR}"
    )

    # --------------------------------------------------------
    # Camera frame publisher
    #
    # Named r1_vision_node because Unitree SDK2 already
    # defines its own "vision_node" target.
    # --------------------------------------------------------

    add_executable(
        r1_vision_node
        src/vision_node.cc
    )

    target_link_libraries(
        r1_vision_node
        PRIVATE
            r1_vision_common
    )

    # --------------------------------------------------------
    # Motion controller
    #
    # Reads YOLO detections and controls R1 head.
    # fmt is required by Unitree ArmSdk / spdlog.
    # --------------------------------------------------------

    add_executable(
        r1_motion_node
        src/motion_node.cc
    )

    target_link_libraries(
        r1_motion_node
        PRIVATE
            r1_vision_common
            fmt::fmt
    )

    # --------------------------------------------------------
    # Hand / camera calibration
    # --------------------------------------------------------

    add_executable(
        r1_hand_calibrate
        src/r1_hand_calibrate.cc
    )

    target_link_libraries(
        r1_hand_calibrate
        PRIVATE
            r1_vision_common
    )

endif()

# ============================================================
# Voice
# ============================================================

if(BUILD_VOICE)

    find_package(CURL REQUIRED)

    # --------------------------------------------------------
    # Voice assistant
    # --------------------------------------------------------

    add_executable(
        r1_voice
        src/voice_node.cc
        src/stt.cc
        src/llm.cc
        src/tts.cc
        src/conversation.cc
        src/vision_context.cc
    )

    target_include_directories(
        r1_voice
        PRIVATE
            "${CMAKE_CURRENT_SOURCE_DIR}/include"
            "${UNITREE_SDK2_DIR}/include"
    )

    target_link_libraries(
        r1_voice
        PRIVATE
            unitree_sdk2
            CURL::libcurl
    )

    # --------------------------------------------------------
    # Raw microphone test
    # --------------------------------------------------------

    add_executable(
        r1_mic_test
        src/r1_mic_test.cc
    )

    target_include_directories(
        r1_mic_test
        PRIVATE
            "${CMAKE_CURRENT_SOURCE_DIR}/include"
            "${UNITREE_SDK2_DIR}/include"
    )

    target_link_libraries(
        r1_mic_test
        PRIVATE
            unitree_sdk2
    )

endif()

# ============================================================
# Install
# ============================================================

set(INSTALL_TARGETS "")

if(BUILD_VISION)

    list(
        APPEND INSTALL_TARGETS
            r1_vision_node
            r1_motion_node
            r1_hand_calibrate
    )

endif()

if(BUILD_VOICE)

    list(
        APPEND INSTALL_TARGETS
            r1_voice
            r1_mic_test
    )

endif()

if(INSTALL_TARGETS)

    install(
        TARGETS
            ${INSTALL_TARGETS}
        RUNTIME DESTINATION bin
    )

endif()

# ============================================================
# Install configuration / scripts
# ============================================================

if(BUILD_VISION)

    install(
        DIRECTORY
            config/
        DESTINATION
            share/r1_vision/config
    )

    install(
        PROGRAMS
            scripts/yolo_detector.py
        DESTINATION
            share/r1_vision/scripts
    )

endif()