cmake_minimum_required(VERSION 3.8)
project(ground_slam)

# Default to C++17 (ROS2 Humble standard)
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

# ── ROS2 packages ──────────────────────────────────────
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(visualization_msgs REQUIRED)

# ── System dependencies ────────────────────────────────
find_package(OpenCV 4.2 REQUIRED)
find_package(OpenMP REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(FFTW3 REQUIRED)
find_package(Ceres REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(Boost REQUIRED)
find_package(VTK REQUIRED)

# ── Compiler flags ─────────────────────────────────────
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native -Wno-reorder")
if(OPENMP_FOUND)
  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()

# ── Include directories ────────────────────────────────
include_directories(
  ${PROJECT_SOURCE_DIR}/include
  ${OpenCV_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIR}
  ${FFTW3_INCLUDE_DIRS}
  ${YAML_CPP_INCLUDE_DIR}
  ${CERES_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${VTK_INCLUDE_DIRS}
)

# ── Core library ───────────────────────────────────────
add_library(${PROJECT_NAME}_lib SHARED
  src/optimization_2d/pose_graph_2d.cc
  src/utils.cc
  src/dataset.cc
  src/camera.cc
  src/frame.cc
  src/edge.cc
  src/correlation_flow.cc
  src/loop_closure.cc
  src/map.cc
  src/map_builder.cc
  src/thread_publisher.cc
  src/map_stitcher.cc
  src/visualization.cc
  src/timer.cc
)

target_link_libraries(${PROJECT_NAME}_lib
  -lfftw3
  -lfftw3f
  ${OpenCV_LIBS}
  ${EIGEN3_LIBS}
  ${CERES_LIBRARIES}
  yaml-cpp
  ${Boost_LIBRARIES}
  ${VTK_LIBRARIES}
)

# ROS2 deps (header-only via include_directories above, link where needed)
ament_target_dependencies(${PROJECT_NAME}_lib
  rclcpp
  geometry_msgs
  nav_msgs
  std_msgs
  sensor_msgs
  cv_bridge
  tf2
  tf2_geometry_msgs
  tf2_ros
  visualization_msgs
)

# ── Executable ─────────────────────────────────────────
add_executable(${PROJECT_NAME} main.cpp)
target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib)

ament_target_dependencies(${PROJECT_NAME}
  rclcpp
)

# ── Install ────────────────────────────────────────────
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_lib
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY configs/
  DESTINATION share/${PROJECT_NAME}/configs
)

ament_package()