find_package(Eigen3 3.3 REQUIRED NO_MODULE)
find_package(osqp REQUIRED)
find_package(OsqpEigen REQUIRED)
find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED)
pkg_check_modules(TINYXML2 REQUIRED tinyxml2)

include_directories(${TINYXML2_INCLUDE_DIRS})

add_library(planner STATIC
    joint_space_planner/src/joint_space_planner.cpp
    joint_space_planner/src/joint_space_planner_creator.cpp
        joint_space_planner/src/toppra_bspline.cpp
)

target_include_directories(planner PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

include_directories(
        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/include
)


link_directories(
        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
)

target_link_libraries(planner PRIVATE
        Eigen3::Eigen
        OsqpEigen::OsqpEigen
        ${TINYXML2_LIBRARIES}
        fcl
        toppra
)

add_library(cmvr_es::planner ALIAS planner)



# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
find_package(glog REQUIRED)
find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED)
find_package(OpenCV REQUIRED)

include_directories(
        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)


link_directories(
        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)




add_executable(joint_space_planner_test
        ${CMAKE_CURRENT_SOURCE_DIR}/joint_space_planner/src/joint_space_planner_test.cpp
)


target_link_libraries(joint_space_planner_test
        PRIVATE
        gtest
        gtest_main
        pthread
        glog::glog
        proto-objects
        ccd
        fcl
        ${OpenCV_LIBS}
        Eigen3::Eigen
        OsqpEigen::OsqpEigen
        cmvr_es::utils
        cmvr_es::data_center
        cmvr_es::planner
)