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(ik_solver STATIC
    ik_solver/src/ik_solver.cpp
    ik_solver/src/ik_solver_creator.cpp
        opt_psi_limit_bias_solver/src/bias_srs_ik_slover.cpp
        opt_psi_limit_bias_solver/src/joints_limit_analyzer.cpp
        opt_psi_limit_bias_solver/src/opt_psi_limit_bias_solver.cpp
        opt_psi_limit_bias_solver/src/opt_psi_selector.cpp
)

target_include_directories(ik_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

target_link_libraries(ik_solver PRIVATE
        Eigen3::Eigen
        OsqpEigen::OsqpEigen
        ${TINYXML2_LIBRARIES}
        fcl
        cmvr_es::data_center
)

add_library(cmvr_es::ik_solver ALIAS ik_solver)


# --------------------------------------------------------
# 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
)

include_directories(
        ${CMAKE_SOURCE_DIR}/third_party/manif/0.0.5/include
)


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




add_executable(srs_ik_test
        ${CMAKE_CURRENT_SOURCE_DIR}/opt_psi_limit_bias_solver/src/srs_ik_test.cpp
)


target_link_libraries(srs_ik_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::ik_solver
)