cmake_minimum_required(VERSION 3.8)
project(mpc_cbf_controller)

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

set(CMAKE_CXX_STANDARD 17)

find_package(ament_cmake REQUIRED)
find_package(mpc_cbf_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(mpc_cbf_interfaces REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(casadi REQUIRED)

include_directories(include ${EIGEN3_INCLUDE_DIR})

# MPC-CBF library
add_library(mpc_cbf_solver_lib
  src/mpc_cbf_solver.cpp
)
target_link_libraries(mpc_cbf_solver_lib casadi)
ament_target_dependencies(mpc_cbf_solver_lib Eigen3)

# MPC-CBF node
add_executable(mpc_cbf_node src/mpc_cbf_node.cpp)
target_link_libraries(mpc_cbf_node mpc_cbf_solver_lib)
ament_target_dependencies(mpc_cbf_node
  rclcpp geometry_msgs nav_msgs visualization_msgs
  tf2 tf2_ros mpc_cbf_interfaces
)

install(TARGETS mpc_cbf_node
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY include/
  DESTINATION include
)

install(DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  find_package(mpc_cbf_interfaces REQUIRED)
  
  ament_add_gtest(test_mpc_solver test/test_solver.cpp)
  target_link_libraries(test_mpc_solver mpc_cbf_solver_lib)
  ament_target_dependencies(test_mpc_solver rclcpp Eigen3)
endif()

ament_package()
