diff --git a/CMakeLists.txt b/CMakeLists.txt
index de96fce..09894d9 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,114 +1,150 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_voxel_grid VERSION 1.0.0 LANGUAGES CXX)
 
-# ========================================================
-# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
-# ========================================================
-
-# Detect if building with Catkin
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
   set(BUILDING_WITH_CATKIN TRUE)
   message(STATUS "Building robot_voxel_grid with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_voxel_grid with Standalone CMake")
 endif()
 
-project(robot_voxel_grid)
-
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-include(CTest)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
-# ========================================================
 # Find dependencies
-# ========================================================
+find_package(GTest QUIET)
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED)
-endif()
+if (NOT BUILDING_WITH_CATKIN)
 
-# # Check for sys/time.h
-# check_include_file(sys/time.h HAVE_SYS_TIME_H)
-# if (HAVE_SYS_TIME_H)
-#   add_definitions(-DHAVE_SYS_TIME_H)
-# endif (HAVE_SYS_TIME_H)
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+else()
 
 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED)
 
-if(BUILDING_WITH_CATKIN)
   catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES robot_voxel_grid
+    LIBRARIES ${PROJECT_NAME}
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# Include directories
-include_directories(  
-  include
-  ${GTEST_INCLUDE_DIRS}
-  )
+# Libraries
+add_library(${PROJECT_NAME} SHARED
+  src/voxel_grid.cpp
+)
 
 if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-# Library
-add_library(robot_voxel_grid SHARED src/voxel_grid.cpp)
-
-target_include_directories(robot_voxel_grid
+  target_include_directories(${PROJECT_NAME}
     PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
-)
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+  )
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
 
 if(BUILDING_WITH_CATKIN)
-  add_dependencies(robot_voxel_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()
-
-# ========================================================
-# Installation
-# ========================================================
-
-# Install targets
-install(TARGETS robot_voxel_grid
-  EXPORT robot_voxel_gridTargets
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
-)
-
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers (standalone)
-  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION include/${PROJECT_NAME}
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
   )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: GTest (optional)")
+  message(STATUS "=================================")
 endif()
 
-install(EXPORT robot_voxel_gridTargets
-  DESTINATION lib/cmake/robot_voxel_grid)
-
-
-# Testing
-enable_testing()
-
-# Find Google Test
-find_package(GTest QUIET)
+# ========================================================
+# Test executables
+# ========================================================
 if(GTest_FOUND OR GTEST_FOUND)
-  # Find pthread (required for GTest)
+  enable_testing()
   find_package(Threads REQUIRED)
   
-  include_directories(${GTEST_INCLUDE_DIRS})
-  add_executable(robot_voxel_grid_tests test/voxel_grid_tests.cpp)
-  target_link_libraries(robot_voxel_grid_tests
-    robot_voxel_grid
-    ${GTEST_LIBRARIES}
-    Threads::Threads
-  )
-  add_test(NAME robot_voxel_grid_tests COMMAND robot_voxel_grid_tests)
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/voxel_grid_tests.cpp)
+    add_executable(${PROJECT_NAME}_test test/voxel_grid_tests.cpp)
+    target_link_libraries(${PROJECT_NAME}_test PRIVATE 
+      ${PROJECT_NAME}
+      GTest::GTest
+      GTest::Main
+      Threads::Threads
+    )
+    add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test)
+  endif()
 else()
   message(STATUS "Google Test not found. Tests will not be built.")
 endif()