update tracy to v0.13.1

- uses a huge global seen-states-cache. Not scalable without more
filtering

.clang-format

@@ -0,0 +1,56 @@
+---
+Language: Cpp
+BasedOnStyle: LLVM
+AccessModifierOffset: -4
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignOperands: false
+AlignTrailingComments: true
+AllowShortBlocksOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: false
+AlwaysBreakTemplateDeclarations: Yes
+BraceWrapping:
+  AfterCaseLabel: true
+  AfterClass: true
+  AfterControlStatement: false
+  AfterEnum: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+  AfterUnion: true
+  AfterExternBlock: false
+  BeforeCatch: true
+  BeforeElse: false
+  BeforeLambdaBody: true
+  BeforeWhile: false
+  SplitEmptyFunction: false
+  SplitEmptyRecord: false
+  SplitEmptyNamespace: false
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+IncludeCategories:
+  - Regex: '^<.*'
+    Priority: 1
+  - Regex: '^".*'
+    Priority: 2
+  - Regex: '.*'
+    Priority: 3
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IndentCaseBlocks: true
+IndentGotoLabels: false
+IndentPPDirectives: BeforeHash
+IndentWidth: 4
+InsertNewlineAtEOF: true
+MacroBlockBegin: ''
+MacroBlockEnd: ''
+PointerAlignment: Left
+SpaceInEmptyParentheses: false
+SpacesInAngles: false
+SpacesInConditionalStatement: false
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+TabWidth: 4
+...

.clang-tidy

@@ -0,0 +1,145 @@
+# Generated from CLion Inspection settings
+---
+Checks: '-*,
+bugprone-argument-comment,
+bugprone-assert-side-effect,
+bugprone-bad-signal-to-kill-thread,
+bugprone-branch-clone,
+bugprone-copy-constructor-init,
+bugprone-dangling-handle,
+bugprone-dynamic-static-initializers,
+bugprone-fold-init-type,
+bugprone-forward-declaration-namespace,
+bugprone-forwarding-reference-overload,
+bugprone-inaccurate-erase,
+bugprone-incorrect-roundings,
+bugprone-integer-division,
+bugprone-lambda-function-name,
+bugprone-macro-parentheses,
+bugprone-macro-repeated-side-effects,
+bugprone-misplaced-operator-in-strlen-in-alloc,
+bugprone-misplaced-pointer-arithmetic-in-alloc,
+bugprone-misplaced-widening-cast,
+bugprone-move-forwarding-reference,
+bugprone-multiple-statement-macro,
+bugprone-no-escape,
+bugprone-parent-virtual-call,
+bugprone-posix-return,
+bugprone-reserved-identifier,
+bugprone-sizeof-container,
+bugprone-sizeof-expression,
+bugprone-spuriously-wake-up-functions,
+bugprone-string-constructor,
+bugprone-string-integer-assignment,
+bugprone-string-literal-with-embedded-nul,
+bugprone-suspicious-enum-usage,
+bugprone-suspicious-include,
+bugprone-suspicious-memset-usage,
+bugprone-suspicious-missing-comma,
+bugprone-suspicious-semicolon,
+bugprone-suspicious-string-compare,
+bugprone-suspicious-memory-comparison,
+bugprone-suspicious-realloc-usage,
+bugprone-swapped-arguments,
+bugprone-terminating-continue,
+bugprone-throw-keyword-missing,
+bugprone-too-small-loop-variable,
+bugprone-undefined-memory-manipulation,
+bugprone-undelegated-constructor,
+bugprone-unhandled-self-assignment,
+bugprone-unused-raii,
+bugprone-unused-return-value,
+bugprone-use-after-move,
+bugprone-virtual-near-miss,
+cert-dcl21-cpp,
+cert-dcl58-cpp,
+cert-err34-c,
+cert-err52-cpp,
+cert-err60-cpp,
+cert-flp30-c,
+cert-msc50-cpp,
+cert-msc51-cpp,
+cert-str34-c,
+cppcoreguidelines-interfaces-global-init,
+cppcoreguidelines-narrowing-conversions,
+cppcoreguidelines-pro-type-member-init,
+cppcoreguidelines-pro-type-static-cast-downcast,
+cppcoreguidelines-slicing,
+google-default-arguments,
+google-runtime-operator,
+hicpp-exception-baseclass,
+hicpp-multiway-paths-covered,
+misc-misplaced-const,
+misc-new-delete-overloads,
+misc-non-copyable-objects,
+misc-throw-by-value-catch-by-reference,
+misc-unconventional-assign-operator,
+misc-uniqueptr-reset-release,
+modernize-avoid-bind,
+modernize-concat-nested-namespaces,
+modernize-deprecated-headers,
+modernize-deprecated-ios-base-aliases,
+modernize-loop-convert,
+modernize-make-shared,
+modernize-make-unique,
+modernize-pass-by-value,
+modernize-raw-string-literal,
+modernize-redundant-void-arg,
+modernize-replace-auto-ptr,
+modernize-replace-disallow-copy-and-assign-macro,
+modernize-replace-random-shuffle,
+modernize-return-braced-init-list,
+modernize-shrink-to-fit,
+modernize-unary-static-assert,
+modernize-use-auto,
+modernize-use-bool-literals,
+modernize-use-emplace,
+modernize-use-equals-default,
+modernize-use-equals-delete,
+modernize-use-nodiscard,
+modernize-use-noexcept,
+modernize-use-nullptr,
+modernize-use-override,
+modernize-use-transparent-functors,
+modernize-use-uncaught-exceptions,
+mpi-buffer-deref,
+mpi-type-mismatch,
+openmp-use-default-none,
+performance-faster-string-find,
+performance-for-range-copy,
+performance-implicit-conversion-in-loop,
+performance-inefficient-algorithm,
+performance-inefficient-string-concatenation,
+performance-inefficient-vector-operation,
+performance-move-const-arg,
+performance-move-constructor-init,
+performance-no-automatic-move,
+performance-noexcept-move-constructor,
+performance-trivially-destructible,
+performance-type-promotion-in-math-fn,
+performance-unnecessary-copy-initialization,
+performance-unnecessary-value-param,
+portability-simd-intrinsics,
+readability-avoid-const-params-in-decls,
+readability-const-return-type,
+readability-container-size-empty,
+readability-convert-member-functions-to-static,
+readability-delete-null-pointer,
+readability-deleted-default,
+readability-inconsistent-declaration-parameter-name,
+readability-make-member-function-const,
+readability-misleading-indentation,
+readability-misplaced-array-index,
+readability-non-const-parameter,
+readability-redundant-control-flow,
+readability-redundant-declaration,
+readability-redundant-function-ptr-dereference,
+readability-redundant-smartptr-get,
+readability-redundant-string-cstr,
+readability-redundant-string-init,
+readability-simplify-subscript-expr,
+readability-static-accessed-through-instance,
+readability-static-definition-in-anonymous-namespace,
+readability-string-compare,
+readability-uniqueptr-delete-release,
+readability-use-anyofallof'

.gitignore

@@ -5,3 +5,9 @@ cmake-build-release
 /result
 /.gdb_history
 /valgrind.log
+.idea
+/perf.data
+/perf.data.old
+/clusters.puzzle
+/benchs.json
+/benchs.old.json

CMakeLists.txt

@@ -1,12 +1,85 @@
-cmake_minimum_required(VERSION 3.25)
+cmake_minimum_required(VERSION 3.28)
 project(MassSprings)
 
 set(CMAKE_CXX_STANDARD 26)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wfloat-equal -Wundef -Wshadow -Wpointer-arith -Wcast-align -Wno-unused-parameter -Wunreachable-code")
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O2 -ggdb") # -fsanitize=address already fails on InitWindow(), -fsanitize=undefined, -fsanitize=leak
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Ofast -march=native")
+# Disable boost warning because our cmake/boost are recent enough
+if(POLICY CMP0167)
+    cmake_policy(SET CMP0167 NEW)
+endif()
+
+option(DISABLE_THREADPOOL "Disable additional physics threads" OFF)
+option(DISABLE_BACKWARD "Disable backward stacktrace printer" OFF)
+option(DISABLE_TRACY "Disable the Tracy profiler client" OFF)
+option(DISABLE_TESTS "Disable building tests" OFF)
+option(DISABLE_BENCH "Disable building benchmarks" OFF)
+
+# Headers + Sources (excluding main.cpp)
+set(SOURCES
+    src/backward.cpp
+    src/bits.cpp
+    src/cpu_layout_engine.cpp
+    src/cpu_spring_system.cpp
+    src/graph_distances.cpp
+    src/input_handler.cpp
+    src/load_save.cpp
+    src/octree.cpp
+    src/orbit_camera.cpp
+    src/puzzle.cpp
+    src/renderer.cpp
+    src/state_manager.cpp
+    src/user_interface.cpp
+)
+
+# Libraries
+include(FetchContent)
+find_package(raylib REQUIRED)
+find_package(GLEW REQUIRED)
+find_package(libmorton REQUIRED)
+find_package(Boost COMPONENTS program_options REQUIRED)
+set(LIBS raylib GLEW::GLEW Boost::headers Boost::program_options)
+set(FLAGS "")
+
+if(WIN32)
+    list(APPEND LIBS opengl32 gdi32 winmm)
+endif()
+
+if(NOT DISABLE_THREADPOOL)
+    list(APPEND FLAGS THREADPOOL)
+endif()
+
+if(NOT DISABLE_BACKWARD)
+    find_package(Backward REQUIRED)
+
+    list(APPEND LIBS Backward::Backward)
+    list(APPEND FLAGS BACKWARD)
+
+    message("-- BACKWARD: Enabled")
+endif()
+
+if(NOT DISABLE_TRACY)
+    FetchContent_Declare(tracy
+                         GIT_REPOSITORY https://github.com/wolfpld/tracy.git
+                         GIT_TAG v0.13.1
+                         GIT_SHALLOW TRUE
+                         GIT_PROGRESS TRUE
+    )
+    FetchContent_MakeAvailable(tracy)
+    option(TRACY_ENABLE "" ON)
+    option(TRACY_ON_DEMAND "" ON)
+
+    list(APPEND LIBS TracyClient)
+    list(APPEND FLAGS TRACY)
+
+    message("-- TRACY: Enabled")
+endif()
+
+# Set this after fetching tracy to hide tracy's warnings.
+# We set -Wno-alloc-size-larger-than because it prevents BS::thread_pool from building with current gcc
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wfloat-equal -Wundef -Wshadow -Wpointer-arith -Wcast-align -Wno-unused-parameter -Wunreachable-code -Wno-alloc-size-larger-than")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ggdb -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -ggdb -O3 -ffast-math -march=native")
 
 message("-- CMAKE_C_FLAGS: ${CMAKE_C_FLAGS}")
 message("-- CMAKE_C_FLAGS_DEBUG: ${CMAKE_C_FLAGS_DEBUG}")
@@ -15,58 +88,60 @@ message("-- CMAKE_CXX_FLAGS: ${CMAKE_CXX_FLAGS}")
 message("-- CMAKE_CXX_FLAGS_DEBUG: ${CMAKE_CXX_FLAGS_DEBUG}")
 message("-- CMAKE_CXX_FLAGS_RELEASE: ${CMAKE_CXX_FLAGS_RELEASE}")
 
-find_package(raylib REQUIRED)
-find_package(Backward REQUIRED)
-
-# Headers + Sources
-include_directories(include)
-set(SOURCES
-    src/main.cpp
-    src/camera.cpp
-    src/renderer.cpp
-    src/octree.cpp
-    src/physics.cpp
-    src/puzzle.cpp
-    src/state.cpp
-    src/input.cpp
-    src/tracy.cpp
-    src/backward.cpp
-    src/distance.cpp
-)
-
 # Main target
-add_executable(masssprings ${SOURCES})
-target_include_directories(masssprings PUBLIC ${RAYLIB_CPP_INCLUDE_DIR})
-target_link_libraries(masssprings PUBLIC raylib Backward::Backward)
+add_executable(masssprings src/main.cpp ${SOURCES})
+target_include_directories(masssprings PRIVATE include)
+target_link_libraries(masssprings PRIVATE ${LIBS})
+target_compile_definitions(masssprings PRIVATE ${FLAGS})
 
-# Tracy target
-if(USE_TRACY)
-    include(FetchContent)
-    FetchContent_Declare(tracy
-        GIT_REPOSITORY https://github.com/wolfpld/tracy.git
-        GIT_TAG v0.11.1
-        GIT_SHALLOW TRUE
-        GIT_PROGRESS TRUE
+# Testing
+if(NOT DISABLE_TESTS AND NOT WIN32)
+    enable_testing()
+
+    FetchContent_Declare(Catch2
+                         GIT_REPOSITORY https://github.com/catchorg/Catch2.git
+                         GIT_TAG v3.13.0
     )
-    FetchContent_MakeAvailable(tracy)
-    option(TRACY_ENABLE "" ON)
-    option(TRACY_ON_DEMAND "" ON)
+    FetchContent_MakeAvailable(Catch2)
 
-    add_executable(masssprings_tracy ${SOURCES})
-    target_include_directories(masssprings_tracy PUBLIC ${RAYLIB_CPP_INCLUDE_DIR})
-    target_compile_definitions(masssprings_tracy PRIVATE TRACY)
-    target_link_libraries(masssprings_tracy PUBLIC raylib Backward::Backward TracyClient)
+    set(TEST_SOURCES
+        test/bits.cpp
+        test/bitmap.cpp
+        test/bitmap_find_first_empty.cpp
+        # test/puzzle.cpp
+    )
+
+    add_executable(tests ${TEST_SOURCES} ${SOURCES})
+    target_include_directories(tests PRIVATE include)
+    target_link_libraries(tests Catch2::Catch2WithMain raylib GLEW::GLEW)
+
+    include(Catch)
+    catch_discover_tests(tests)
+
+    message("-- TESTS: Enabled")
+endif()
+
+# Benchmarking
+if(NOT DISABLE_BENCH AND NOT WIN32)
+    find_package(benchmark REQUIRED)
+
+    set(BENCH_SOURCES
+        benchmark/state_space.cpp
+    )
+
+    add_executable(benchmarks ${BENCH_SOURCES} ${SOURCES})
+    target_include_directories(benchmarks PRIVATE include)
+    target_link_libraries(benchmarks benchmark raylib GLEW::GLEW)
+
+    message("-- BENCHMARKS: Enabled")
 endif()
 
 # LTO
 include(CheckIPOSupported)
 check_ipo_supported(RESULT supported OUTPUT error)
 if(supported)
-    message(STATUS "IPO / LTO enabled")
+    message("-- IPO/LTO: Enabled")
     set_property(TARGET masssprings PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
-    if(USE_TRACY)
-        set_property(TARGET masssprings_tracy PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
-    endif()
 else()
-    message(STATUS "IPO / LTO not supported: <${error}>")
+    message("-- IPO/LTO: Disabled")
 endif()

README.md

@@ -0,0 +1,16 @@
+# MassSprings - Puzzle Board State Space Explorer
+
+All combinations of pieces reachable from an initial puzzle are explored, the resulting puzzle state-space is visualized as a force-directed graph.
+The graph layout is calculated iteratively using a mass-spring-system with additional pairwise repulsive forces simulated using Barnes-Hut.
+
+![](screenshot.png)
+
+## Running
+
+Requirements:
+
+- Directory `fonts`
+- Directory `shader`
+- Preset file `default.puzzle` (optional)
+
+Run `nix run git+https://gitea.local.chriphost.de/christoph/cpp-masssprings` from the working directory containing the listed requirements.

benchmark/state_space.cpp

@@ -0,0 +1,212 @@
+// ReSharper disable CppTooWideScope
+// ReSharper disable CppDFAUnreadVariable
+#include "puzzle.hpp"
+
+#include <random>
+#include <unordered_set>
+#include <benchmark/benchmark.h>
+#include <boost/unordered/unordered_flat_map.hpp>
+
+static std::vector<std::string> puzzles = {
+    // 0: RushHour 1
+    "S:[6x6] G:[4,2] M:[R] B:[{3x1 _ _ _ _ 1x3} {_ _ _ _ _ _} {_ _ 1x2 2X1 _ _} {_ _ _ 1x2 2x1 _} {1x2 _ 1x2 _ 2x1 _} {_ _ _ 3x1 _ _}]",
+    // 1: RushHour 2
+    "S:[6x6] G:[4,2] M:[R] B:[{1x2 3x1 _ _ 1x2 1x3} {_ 3x1 _ _ _ _} {2X1 _ 1x2 1x2 1x2 _} {2x1 _ _ _ _ _} {_ _ _ 1x2 2x1 _} {_ _ _ _ 2x1 _}]",
+    // 2: RushHour 3
+    "S:[6x6] G:[4,2] M:[R] B:[{3x1 _ _ 1x2 _ _} {1x2 2x1 _ _ _ 1x2} {_ 2X1 _ 1x2 1x2 _} {2x1 _ 1x2 _ _ 1x2} {_ _ _ 2x1 _ _} {_ 2x1 _ 2x1 _ _}]",
+    // 3: RushHour 4
+    "S:[6x6] G:[4,2] M:[R] B:[{1x3 2x1 _ _ 1x2 _} {_ 1x2 1x2 _ _ 1x3} {_ _ _ 2X1 _ _} {3x1 _ _ 1x2 _ _} {_ _ 1x2 _ 2x1 _} {2x1 _ _ 2x1 _ _}]",
+    // 4: RushHour + Walls 1
+    "S:[6x6] G:[4,2] M:[R] B:[{1x2 2x1 _ 1*1 _ _} {_ _ _ 1x2 2x1 _} {1x2 2X1 _ _ _ _} {_ _ 1x2 2x1 _ 1x3} {2x1 _ _ _ _ _} {2x1 _ 3x1 _ _ _}]",
+    // 5: RushHour + Walls 2
+    "S:[6x6] G:[4,2] M:[R] B:[{2x1 _ _ 1x2 1x2 1*1} {3x1 _ _ _ _ _} {1x2 2X1 _ 1x2 _ _} {_ _ 1x2 _ 2x1 _} {_ _ _ 2x1 _ 1x2} {_ _ 2x1 _ 1*1 _}]",
+    // 6: Dad's Puzzler
+    "S:[4x5] G:[0,3] M:[F] B:[{2X2 _ 2x1 _} {_ _ 2x1 _} {1x1 1x1 _ _} {1x2 1x2 2x1 _} {_ _ 2x1 _}]",
+    // 7: Nine Blocks
+    "S:[4x5] G:[0,3] M:[F] B:[{1x2 1x2 _ _} {_ _ 2x1 _} {1x2 1x2 2x1 _} {_ _ 2X2 _} {1x1 1x1 _ _}]",
+    // 8: Quzzle
+    "S:[4x5] G:[2,0] M:[F] B:[{2X2 _ 2x1 _} {_ _ 1x2 1x2} {_ _ _ _} {1x2 2x1 _ 1x1} {_ 2x1 _ 1x1}]",
+    // 9: Thin Klotski
+    "S:[4x5] G:[1,4] M:[F] B:[{1x2 _ 2X1 _} {_ 2x2 _ 1x1} {_ _ _ 1x1} {2x2 _ 1x1 1x1} {_ _ 1x1 1x1}]",
+    // 10: Fat Klotski
+    "S:[4x5] G:[1,3] M:[F] B:[{_ 2X2 _ 1x1} {1x1 _ _ 1x2} {1x1 2x2 _ _} {1x1 _ _ _} {1x1 1x1 2x1 _}]",
+    // 11: Klotski
+    "S:[4x5] G:[1,3] M:[F] B:[{1x2 2X2 _ 1x2} {_ _ _ _} {1x2 2x1 _ 1x2} {_ 1x1 1x1 _} {1x1 _ _ 1x1}]",
+    // 12: Century
+    "S:[4x5] G:[1,3] M:[F] B:[{1x1 2X2 _ 1x1} {1x2 _ _ 1x2} {_ 1x2 _ _} {1x1 _ _ 1x1} {2x1 _ 2x1 _}]",
+    // 13: Super Century
+    "S:[4x5] G:[1,3] M:[F] B:[{1x2 1x1 1x1 1x1} {_ 1x2 2X2 _} {1x2 _ _ _} {_ 2x1 _ 1x1} {_ 2x1 _ _}]",
+    // 14: Supercompo
+    "S:[4x5] G:[1,3] M:[F] B:[{_ 2X2 _ _} {1x1 _ _ 1x1} {1x2 2x1 _ 1x2} {_ 2x1 _ _} {1x1 2x1 _ 1x1}]",
+};
+
+template <u8 N>
+struct uint_hasher
+{
+    int64_t nums;
+
+    auto operator()(const std::array<u64, N>& ints) const noexcept -> size_t
+    {
+        size_t h = 0;
+        for (size_t i = 0; i < N; ++i) {
+            puzzle::hash_combine(h, ints[i]);
+        }
+        return h;
+    }
+};
+
+template <u8 N>
+static auto unordered_set_uint64(benchmark::State& state) -> void
+{
+    std::random_device random_device;
+    std::mt19937 generator(random_device());
+    std::uniform_int_distribution<u64> distribution(
+        std::numeric_limits<u64>::min(),
+        std::numeric_limits<u64>::max()
+    );
+
+    std::unordered_set<std::array<u64, N>, uint_hasher<N>> set;
+    std::array<u64, N> ints;
+    for (size_t i = 0; i < N; ++i) {
+        ints[i] = distribution(generator);
+    }
+
+    for (auto _ : state) {
+        for (size_t i = 0; i < 100000; ++i) {
+            set.emplace(ints);
+        }
+
+        benchmark::DoNotOptimize(set);
+    }
+}
+
+template <u8 N>
+static auto unordered_flat_set_uint64(benchmark::State& state) -> void
+{
+    std::random_device random_device;
+    std::mt19937 generator(random_device());
+    std::uniform_int_distribution<u64> distribution(
+        std::numeric_limits<u64>::min(),
+        std::numeric_limits<u64>::max()
+    );
+
+    boost::unordered_flat_set<std::array<u64, N>, uint_hasher<N>> set;
+    std::array<u64, N> ints;
+    for (size_t i = 0; i < N; ++i) {
+        ints[i] = distribution(generator);
+    }
+
+    for (auto _ : state) {
+        for (size_t i = 0; i < 100000; ++i) {
+            set.emplace(ints);
+        }
+
+        benchmark::DoNotOptimize(set);
+    }
+}
+
+static auto unordered_flat_set_block_hasher(benchmark::State& state) -> void
+{
+    blockset set;
+    const block b = block(2, 3, 1, 2, true, false);
+
+    for (auto _ : state) {
+        for (size_t i = 0; i < 100000; ++i) {
+            set.emplace(b);
+        }
+
+        benchmark::DoNotOptimize(set);
+    }
+}
+
+static auto unordered_flat_set_block_hasher2(benchmark::State& state) -> void
+{
+    blockset2 set;
+    const block b = block(2, 3, 1, 2, true, false);
+
+    for (auto _ : state) {
+        for (size_t i = 0; i < 100000; ++i) {
+            set.emplace(b);
+        }
+
+        benchmark::DoNotOptimize(set);
+    }
+}
+
+static auto unordered_flat_set_puzzle_hasher(benchmark::State& state) -> void
+{
+    puzzleset set;
+    const puzzle p = puzzle(puzzles[0]);
+
+    for (auto _ : state) {
+        for (size_t i = 0; i < 100000; ++i) {
+            set.emplace(p);
+        }
+
+        benchmark::DoNotOptimize(set);
+    }
+}
+
+static auto explore_state_space(benchmark::State& state) -> void
+{
+    const puzzle p = puzzle(puzzles[state.range(0)]);
+
+    for (auto _ : state) {
+        auto space = p.explore_state_space();
+
+        benchmark::DoNotOptimize(space);
+    }
+}
+
+static auto explore_rush_hour_puzzle_space(benchmark::State& state) -> void
+{
+    constexpr u8 max_blocks = 5;
+
+    constexpr u8 board_width = 4;
+    constexpr u8 board_height = 5;
+    constexpr u8 goal_x = board_width - 1;
+    constexpr u8 goal_y = 2;
+    constexpr bool restricted = true;
+
+    const blockset2 permitted_blocks = {
+        block(0, 0, 2, 1, false, false),
+        block(0, 0, 3, 1, false, false),
+        block(0, 0, 1, 2, false, false),
+        block(0, 0, 1, 3, false, false)
+    };
+    const block target_block = block(0, 0, 2, 1, true, false);
+    constexpr std::tuple<u8, u8, u8, u8> target_block_pos_range = {
+        0,
+        goal_y,
+        goal_x,
+        goal_y
+    };
+
+    const puzzle p = puzzle(board_width, board_height, goal_x, goal_y, restricted, true);
+
+    for (auto _ : state) {
+        puzzleset result = p.explore_puzzle_space(
+            permitted_blocks,
+            target_block,
+            target_block_pos_range,
+            max_blocks,
+            0,
+            std::nullopt);
+
+        benchmark::DoNotOptimize(result);
+    }
+}
+
+BENCHMARK(unordered_set_uint64<4>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_set_uint64<8>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_set_uint64<16>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_uint64<4>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_uint64<8>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_uint64<16>)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_block_hasher)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_block_hasher2)->Unit(benchmark::kMicrosecond);
+BENCHMARK(unordered_flat_set_puzzle_hasher)->Unit(benchmark::kMicrosecond);
+BENCHMARK(explore_state_space)->DenseRange(0, puzzles.size() - 1)->Unit(benchmark::kMicrosecond);
+BENCHMARK(explore_rush_hour_puzzle_space)->Unit(benchmark::kSecond);
+
+BENCHMARK_MAIN();

default.puzzle

@@ -1,56 +1,30 @@
-# 1 Block, 1 Axis, no goal
-R459912......................................
-
-# 2 Blocks, 2 Axes, no goal
-R45991212....................................
-
-# 3 Blocks, 3 Axes, still no goal
-R4599121212..................................
-
-# Square with no goal
-F449921....111222..11......111111....
-
 # RushHour 1
-R664231........13................12ba..........1221..12..12..21........31....
-
+S:[6x6] G:[4,2] M:[R] B:[{3x1 _ _ _ _ 1x3} {_ _ _ _ _ _} {_ _ 1x2 2X1 _ _} {_ _ _ 1x2 2x1 _} {1x2 _ 1x2 _ 2x1 _} {_ _ _ 3x1 _ _}]
 # RushHour 2
-R66421231....1213..31........ba..121212..21................1221..........21..
-
+S:[6x6] G:[4,2] M:[R] B:[{1x2 3x1 _ _ 1x2 1x3} {_ 3x1 _ _ _ _} {2X1 _ 1x2 1x2 1x2 _} {2x1 _ _ _ _ _} {_ _ _ 1x2 2x1 _} {_ _ _ _ 2x1 _}]
 # RushHour 3
-R664231....12....1221......12..ba..1212..21..12....12......21......21..21....
-
+S:[6x6] G:[4,2] M:[R] B:[{3x1 _ _ 1x2 _ _} {1x2 2x1 _ _ _ 1x2} {_ 2X1 _ 1x2 1x2 _} {2x1 _ 1x2 _ _ 1x2} {_ _ _ 2x1 _ _} {_ 2x1 _ 2x1 _ _}]
 # RushHour 4
-R66421321....12....1212....13......ba....31....12........12..21..21....21....
-
+S:[6x6] G:[4,2] M:[R] B:[{1x3 2x1 _ _ 1x2 _} {_ 1x2 1x2 _ _ 1x3} {_ _ _ 2X1 _ _} {3x1 _ _ 1x2 _ _} {_ _ 1x2 _ 2x1 _} {2x1 _ _ 2x1 _ _}]
 # RushHour + Walls 1
-R66421221..AA..........1221..12ba............1221..1321..........21..31......
-
+S:[6x6] G:[4,2] M:[R] B:[{1x2 2x1 _ 1*1 _ _} {_ _ _ 1x2 2x1 _} {1x2 2X1 _ _ _ _} {_ _ 1x2 2x1 _ 1x3} {2x1 _ _ _ _ _} {2x1 _ 3x1 _ _ _}]
 # RushHour + Walls 2
-R664221....1212AA31..........12ba..12........12..21........21..12....21..AA..
-
+S:[6x6] G:[4,2] M:[R] B:[{2x1 _ _ 1x2 1x2 1*1} {3x1 _ _ _ _ _} {1x2 2X1 _ 1x2 _ _} {_ _ 1x2 _ 2x1 _} {_ _ _ 2x1 _ 1x2} {_ _ 2x1 _ 1*1 _}]
 # Dad's Puzzler
-F4503bb..21......21..1111....121221......21..
-
-# Nine Block (Worse)
-F45031212........21..121221......bb..1111....
-
+S:[4x5] G:[0,3] M:[F] B:[{2X2 _ 2x1 _} {_ _ 2x1 _} {1x1 1x1 _ _} {1x2 1x2 2x1 _} {_ _ 2x1 _}]
+# Nine Blocks
+S:[4x5] G:[0,3] M:[F] B:[{1x2 1x2 _ _} {_ _ 2x1 _} {1x2 1x2 2x1 _} {_ _ 2X2 _} {1x1 1x1 _ _}]
 # Quzzle
-F4520bb..21......1212........1221..11..21..11
-
+S:[4x5] G:[2,0] M:[F] B:[{2X2 _ 2x1 _} {_ _ 1x2 1x2} {_ _ _ _} {1x2 2x1 _ 1x1} {_ 2x1 _ 1x1}]
 # Thin Klotski
-F451412..ba....22..11......1122..1111....1111
-
-# Klotski
-F451312bb..12........1221..12..1111..11....11
-
+S:[4x5] G:[1,4] M:[F] B:[{1x2 _ 2X1 _} {_ 2x2 _ 1x1} {_ _ _ 1x1} {2x2 _ 1x1 1x1} {_ _ 1x1 1x1}]
 # Fat Klotski
-F4513..bb..1111....121122....11......111121..
-
+S:[4x5] G:[1,3] M:[F] B:[{_ 2X2 _ 1x1} {1x1 _ _ 1x2} {1x1 2x2 _ _} {1x1 _ _ _} {1x1 1x1 2x1 _}]
+# Klotski
+S:[4x5] G:[1,3] M:[F] B:[{1x2 2X2 _ 1x2} {_ _ _ _} {1x2 2x1 _ 1x2} {_ 1x1 1x1 _} {1x1 _ _ 1x1}]
 # Century
-F451311bb..1112....12..12....11....1121..21..
-
+S:[4x5] G:[1,3] M:[F] B:[{1x1 2X2 _ 1x1} {1x2 _ _ 1x2} {_ 1x2 _ _} {1x1 _ _ 1x1} {2x1 _ 2x1 _}]
 # Super Century
-F451312111111..12bb..12........21..11....21..
-
+S:[4x5] G:[1,3] M:[F] B:[{1x2 1x1 1x1 1x1} {_ 1x2 2X2 _} {1x2 _ _ _} {_ 2x1 _ 1x1} {_ 2x1 _ _}]
 # Supercompo
-F4513..bb....11....111221..12..21....1121..11
+S:[4x5] G:[1,3] M:[F] B:[{_ 2X2 _ _} {1x1 _ _ 1x1} {1x2 2x1 _ 1x2} {_ 2x1 _ _} {1x1 2x1 _ 1x1}]

flake.lock

@@ -20,11 +20,11 @@
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1770843696,
-        "narHash": "sha256-LovWTGDwXhkfCOmbgLVA10bvsi/P8eDDpRudgk68HA8=",
+        "lastModified": 1773201692,
+        "narHash": "sha256-NXrKzNMniu4Oam2kAFvqJ3GB2kAvlAFIriTAheaY8hw=",
         "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "2343bbb58f99267223bc2aac4fc9ea301a155a16",
+        "rev": "b6067cc0127d4db9c26c79e4de0513e58d0c40c9",
         "type": "github"
       },
       "original": {

flake.nix

@@ -14,88 +14,162 @@ rec {
   # Create a shell (and possibly package) for each possible system, not only x86_64-linux
     flake-utils.lib.eachDefaultSystem (
       system: let
+        # =========================================================================================
+        # Define pkgs/stdenvs
+        # =========================================================================================
         pkgs = import nixpkgs {
           inherit system;
           config.allowUnfree = true;
           overlays = [];
         };
+
+        clangPkgs = import nixpkgs {
+          inherit system;
+          config.allowUnfree = true;
+          overlays = [];
+
+          # Use this to change the compiler:
+          # - GCC: pkgs.stdenv
+          # - Clang: pkgs.clangStdenv
+          # NixOS packages are built using GCC by default. Using clang requires a full rebuild/redownload.
+          config.replaceStdenv = {pkgs}: pkgs.clangStdenv;
+        };
+
+        windowsPkgs = import nixpkgs {
+          inherit system;
+          config.allowUnfree = true;
+          overlays = [];
+
+          # Use this to cross compile to a different system
+          crossSystem = {
+            config = "x86_64-w64-mingw32";
+          };
+        };
+
         inherit (pkgs) lib stdenv;
 
+        # =========================================================================================
+        # Define shell environment
+        # =========================================================================================
+
+        # Setup the shell when entering the "nix develop" environment (bash script).
+        shellHook = let
+          mkCmakeScript = type: let
+            typeLower = lib.toLower type;
+          in
+            pkgs.writers.writeFish "cmake-${typeLower}.fish" ''
+              cd $FLAKE_PROJECT_ROOT
+
+              echo "Removing build directory ./cmake-build-${typeLower}/"
+              rm -rf ./cmake-build-${typeLower}
+
+              echo "Creating build directory"
+              mkdir cmake-build-${typeLower}
+              cd cmake-build-${typeLower}
+
+              echo "Running cmake"
+              cmake -G "Ninja" \
+                    -DCMAKE_BUILD_TYPE="${type}" \
+                    ..
+
+              echo "Linking compile_commands.json"
+              cd ..
+              ln -sf ./cmake-build-${typeLower}/compile_commands.json ./compile_commands.json
+            '';
+
+          cmakeDebug = mkCmakeScript "Debug";
+          cmakeRelease = mkCmakeScript "Release";
+
+          mkBuildScript = type: let
+            typeLower = lib.toLower type;
+          in
+            pkgs.writers.writeFish "cmake-build.fish" ''
+              cd $FLAKE_PROJECT_ROOT/cmake-build-${typeLower}
+
+              echo "Running cmake"
+              NIX_ENFORCE_NO_NATIVE=0 cmake --build . -j$(nproc)
+            '';
+
+          buildDebug = mkBuildScript "Debug";
+          buildRelease = mkBuildScript "Release";
+
+          # Use this to specify commands that should be ran after entering fish shell
+          initProjectShell = pkgs.writers.writeFish "init-shell.fish" ''
+            echo "Entering \"${description}\" environment..."
+
+            # Determine the project root, used e.g. in cmake scripts
+            set -g -x FLAKE_PROJECT_ROOT (git rev-parse --show-toplevel)
+
+            # C/C++:
+            abbr -a cmake-debug "${cmakeDebug}"
+            abbr -a cmake-release "${cmakeRelease}"
+            abbr -a build-debug "${buildDebug}"
+            abbr -a build-release "${buildRelease}"
+            abbr -a debug-clean "${cmakeDebug} && ${buildDebug} && ./cmake-build-debug/masssprings"
+            abbr -a release-clean "${cmakeRelease} && ${buildRelease} && ./cmake-build-release/masssprings"
+            abbr -a debug "${buildDebug} && ./cmake-build-debug/masssprings"
+            abbr -a release "${buildRelease} && ./cmake-build-release/masssprings"
+
+            abbr -a run "${buildRelease} && ./cmake-build-release/masssprings"
+            abbr -a runclusters "${buildRelease} && ./cmake-build-release/masssprings --output=clusters.puzzle --space=rh --moves=10 --blocks=4"
+            abbr -a runtests "${buildDebug} && ./cmake-build-debug/tests"
+            abbr -a runbenchs "mv -f benchs.json benchs.old.json; ${buildRelease} && sudo cpupower frequency-set --governor performance && ./cmake-build-release/benchmarks --benchmark_out=benchs.json --benchmark_out_format=console; sudo cpupower frequency-set --governor powersave"
+            abbr -a rungdb "${buildDebug} && gdb --tui ./cmake-build-debug/masssprings"
+            abbr -a runvalgrind "${buildDebug} && valgrind --leak-check=full --show-reachable=no --show-leak-kinds=definite,indirect,possible --track-origins=no --suppressions=valgrind.supp --log-file=valgrind.log ./cmake-build-debug/masssprings && cat valgrind.log"
+            abbr -a runperf "${buildRelease} && perf record -g ./cmake-build-release/masssprings && hotspot ./perf.data"
+            abbr -a runperf-graph "${buildRelease} && perf record -g ./cmake-build-release/benchmarks --benchmark_filter='explore_state_space' && hotspot ./perf.data"
+            abbr -a runperf-space "${buildRelease} && perf record -g ./cmake-build-release/benchmarks --benchmark_filter='explore_rush_hour_puzzle_space' && hotspot ./perf.data"
+            abbr -a runtracy "tracy -a 127.0.0.1 &; ${buildRelease} && sudo -E ./cmake-build-release/masssprings"
+
+            abbr -a runclion "clion ./CMakeLists.txt 2>/dev/null 1>&2 & disown;"
+          '';
+        in
+          builtins.concatStringsSep "\n" [
+            # Launch into pure fish shell
+            ''
+              exec "$(type -p fish)" -C "source ${initProjectShell}"
+            ''
+          ];
+
         # ===========================================================================================
         # Define custom dependencies
         # ===========================================================================================
 
-        # 64 bit C/C++ compilers that don't collide (use the same libc)
-        # bintools = pkgs.wrapBintoolsWith {
-        #   bintools = pkgs.bintools.bintools; # Unwrapped bintools
-        #   libc = pkgs.glibc;
-        # };
-        # gcc = lib.hiPrio (pkgs.wrapCCWith {
-        #   cc = pkgs.gcc.cc; # Unwrapped gcc
-        #   libc = pkgs.glibc;
-        #   bintools = bintools;
-        # });
-        # clang = pkgs.wrapCCWith {
-        #   cc = pkgs.clang.cc; # Unwrapped clang
-        #   libc = pkgs.glibc;
-        #   bintools = bintools;
-        # };
+        raygui = stdenv.mkDerivation rec {
+          pname = "raygui";
+          version = "4.0-unstable-2026-02-24";
 
-        # Raylib CPP wrapper
-        # raylib-cpp = stdenv.mkDerivation {
-        #   pname = "raylib-cpp";
-        #   version = "5.5.0-unstable-2025-11-12";
-        #
-        #   src = pkgs.fetchFromGitHub {
-        #     owner = "RobLoach";
-        #     repo = "raylib-cpp";
-        #     rev = "21b0d0f57a09a7f741d20b7157f440ae87f02c76";
-        #     hash = "sha256-P9x6Zc5t648gR7oYXe38PEX/a4oh4PfuVCnjT0vC10k=";
-        #   };
-        #
-        #   # autoPatchelfHook is needed for appendRunpaths
-        #   nativeBuildInputs = with pkgs; [
-        #     cmake
-        #     # autoPatchelfHook
-        #   ];
-        #
-        #   buildInputs = with pkgs; [
-        #     raylib
-        #     glfw
-        #     SDL2
-        #   ];
-        #
-        #   propagatedBuildInputs = with pkgs; [
-        #     libGLU
-        #     libx11
-        #   ];
-        #
-        #   cmakeFlags = [
-        #     "-DBUILD_RAYLIB_CPP_EXAMPLES=OFF"
-        #     "-DBUILD_TESTING=OFF"
-        #     # Point CMake to the nixpkgs raylib so it doesn't try to fetch its own
-        #     "-Draylib_DIR=${pkgs.raylib}/lib/cmake/raylib"
-        #   ];
-        # };
+          src = pkgs.fetchFromGitHub {
+            owner = "raysan5";
+            repo = "raygui";
+            rev = "5788707b6b7000343c14653b1ad3b971ca0597e4";
+            hash = "sha256-wKylPeNw7wO5xuTfnp1OYETQ78EPlr4NU9erbmIFgjE=";
+          };
 
-        # octree = stdenv.mkDerivation {
-        #   pname = "octree";
-        #   version = "2.5-unstable-2025-12-18";
-        #
-        #   src = pkgs.fetchFromGitHub {
-        #     owner = "attcs";
-        #     repo = "octree";
-        #     rev = "5058b3090c8b88e405fe2bfddd6c1c872f2b79d2";
-        #     hash = "sha256-a/aDGQ7cj1GbCjts2s9VEaxyFnL6PF+xJOsSxm9o+4M=";
-        #   };
-        #
-        #   # Header-only library
-        #   dontBuild = true;
-        #   installPhase = ''
-        #     mkdir -p $out/include
-        #     mv ./*.h $out/include/
-        #   '';
-        # };
+          patches = [./raygui.patch];
+
+          dontBuild = true;
+          installPhase = ''
+            runHook preInstall
+
+            mkdir -p $out/{include,lib/pkgconfig}
+
+            install -Dm644 src/raygui.h $out/include/raygui.h
+
+            cat <<EOF > $out/lib/pkgconfig/raygui.pc
+            prefix=$out
+            includedir=$out/include
+
+            Name: raygui
+            Description: Simple and easy-to-use immediate-mode gui library
+            URL: https://github.com/raysan5/raygui
+            Version: ${version}
+            Cflags: -I"{includedir}"
+            EOF
+
+            runHook postInstall
+          '';
+        };
 
         thread-pool = stdenv.mkDerivation {
           pname = "thread-pool";
@@ -111,8 +185,69 @@ rec {
           # Header-only library
           dontBuild = true;
           installPhase = ''
+            runHook preInstall
+
             mkdir -p $out
-            mv ./include $out/include
+            cp -rv ./include $out/include
+
+            runHook postInstall
+          '';
+        };
+
+        # We can use the pkgs.stdenv for Linux+Windows because it's a header only library.
+        # The build is required to create the pkg-config/cmake configuration files.
+        libmorton = stdenv.mkDerivation {
+          pname = "libmorton";
+          version = "0.2.12-unstable-2023-05-24";
+
+          src = pkgs.fetchFromGitHub {
+            owner = "Forceflow";
+            repo = "libmorton";
+            rev = "7923faa88d7e564020b2d5d408bf8c186ecbe363";
+            hash = "sha256-5LHiWu2GIuDmfM2gXGbRsFasE7AmVCSRphNdFElbbjk=";
+          };
+
+          nativeBuildInputs = with pkgs; [cmake];
+
+          cmakeFlags = [
+            "-DBUILD_TESTING=OFF"
+            "-DCMAKE_INSTALL_INCLUDEDIR=include"
+            "-DCMAKE_INSTALL_DATADIR=share"
+          ];
+        };
+
+        glew-windows = windowsPkgs.stdenv.mkDerivation rec {
+          pname = "glew-windows";
+          version = "2.2.0";
+
+          src = pkgs.fetchurl {
+            url = "https://github.com/nigels-com/glew/releases/download/glew-${version}/glew-${version}.tgz";
+            hash = "sha256-1PyCiTz7ABCVeNChojN/uMozWzzsz5e5flzH8I5DU+E=";
+          };
+
+          nativeBuildInputs = with pkgs; [
+            cmake
+            ninja
+            pkg-config
+          ];
+
+          preConfigure = ''
+            cd build/cmake
+          '';
+
+          cmakeFlags = [
+            "-DBUILD_UTILS=OFF"
+            "-DGLEW_OSMESA=OFF"
+            "-DBUILD_SHARED_LIBS=ON"
+            "-DCMAKE_POLICY_VERSION_MINIMUM=3.5"
+          ];
+
+          installPhase = ''
+            runHook preInstall
+
+            cmake --install . --prefix "$out"
+
+            runHook postInstall
           '';
         };
 
@@ -126,25 +261,20 @@ rec {
         # - Those which are needed on $PATH during the build, for example cmake and pkg-config
         # - Setup hooks, for example makeWrapper
         # - Interpreters needed by patchShebangs for build scripts (with the --build flag), which can be the case for e.g. perl
+        # NOTE: Do not add compiler here, they are provided by the stdenv
         nativeBuildInputs = with pkgs; [
           # Languages:
-          binutils
-          gcc
+          # binutils
 
           # C/C++:
+          cmake
+          ninja
           gdb
           valgrind
-          # gnumake
-          cmake
-          # pkg-config
-          # clang-tools
-          # compdb
-          # pprof
-          # gprof2dot
+          kdePackages.kcachegrind
           perf
           hotspot
-          kdePackages.kcachegrind
-          gdbgui
+          # heaptrack
           # renderdoc
         ];
 
@@ -153,22 +283,25 @@ rec {
         # - Interpreters needed by patchShebangs for scripts which are installed, which can be the case for e.g. perl
         buildInputs = with pkgs; [
           # C/C++:
-          # boost
-          # sfml
           raylib
-          # octree # this one doesn't store center of mass per node - which I need :(
-          tracy-wayland
+          raygui
+          glew
           thread-pool
+          libmorton
+          boost
+
+          # Debugging/Testing/Profiling
+          tracy_0_13
           backward-cpp
           libbfd
-          # llvmPackages.openmp # not required for compilation but for clangd to find the headers
-          # raylib-cpp
-          # tinyobjloader
-          # gperftools
+          catch2_3
+          gbenchmark
         ];
+
         # ===========================================================================================
         # Define buildable + installable packages
         # ===========================================================================================
+
         package = stdenv.mkDerivation rec {
           inherit buildInputs;
           pname = "masssprings";
@@ -178,168 +311,161 @@ rec {
           nativeBuildInputs = with pkgs; [
             gcc
             cmake
+
+            # Fix the working directory so the auxiliary files are always available
+            makeWrapper
+          ];
+
+          cmakeFlags = [
+            "-DDISABLE_THREADPOOL=Off"
+            "-DDISABLE_TRACY=On"
+            "-DDISABLE_BACKWARD=On"
+            "-DDISABLE_TESTS=On"
+            "-DDISABLE_BENCH=On"
+          ];
+
+          hardeningDisable = ["all"];
+
+          preConfigure = ''
+            unset NIX_ENFORCE_NO_NATIVE
+          '';
+
+          installPhase = ''
+            runHook preInstall
+
+            mkdir -p $out/lib
+            cp ./${pname} $out/lib/
+            cp -rv $src/default.puzzle $out/lib/
+            cp -rv $src/fonts $out/lib/fonts
+            cp -rv $src/shader $out/lib/shader
+
+            # The wrapper enters the correct working dir, so fonts/shaders/presets are available
+            mkdir -p $out/bin
+            makeWrapper $out/lib/${pname} $out/bin/${pname} --chdir "$out/lib"
+
+            # Generate a .desktop file
+            mkdir -p $out/share/applications
+            cat <<INI > $out/share/applications/${pname}.desktop
+            [Desktop Entry]
+            Terminal=true
+            Name=PuzzleSpaces
+            Exec=$out/bin/${pname} %f
+            Type=Application
+            INI
+
+            runHook postInstall
+          '';
+        };
+
+        windowsPackage = windowsPkgs.stdenv.mkDerivation rec {
+          pname = "masssprings";
+          version = "1.0.0";
+          src = ./.;
+
+          # nativeBuildInputs must be from the build-platform (not cross)
+          # so we use "pkgs" here, not "windowsPkgs"
+          nativeBuildInputs = with pkgs; [
+            cmake
+          ];
+
+          buildInputs = with windowsPkgs; [
+            raylib
+            raygui
+            glew-windows
+            thread-pool
+            libmorton
+
+            # Disable stacktrace since that's platform dependant and won't cross compile to windows
+            # https://github.com/NixOS/nixpkgs/blob/master/pkgs/development/libraries/boost/generic.nix#L43
+            (boost.override {
+              enableShared = false;
+              extraB2Args = [
+                "--without-stacktrace"
+              ];
+            })
+          ];
+
+          cmakeFlags = [
+            "-DCMAKE_SYSTEM_NAME=Windows"
+            "-DDISABLE_THREADPOOL=Off"
+            "-DDISABLE_TRACY=On"
+            "-DDISABLE_BACKWARD=On"
+            "-DDISABLE_TESTS=On"
+            "-DDISABLE_BENCH=On"
           ];
 
           installPhase = ''
+            runHook preInstall
+
             mkdir -p $out/bin
-            cp ./${pname} $out/bin/
-            cp $src/default.puzzle $out/bin/
+            cp -rv ./${pname}.exe $out/bin/
+            cp -rv $src/default.puzzle $out/bin/
+            cp -rv $src/fonts $out/bin/fonts
+            cp -rv $src/shader $out/bin/shader
+
+            runHook postInstall
           '';
         };
       in rec {
-        # Provide package for "nix build"
-        defaultPackage = package;
-        defaultApp = flake-utils.lib.mkApp {
-          drv = defaultPackage;
+        # Provide packages for "nix build" and "nix run"
+        packages = {
+          default = package;
+          windows = windowsPackage;
         };
+        apps.default = flake-utils.lib.mkApp {drv = package;};
 
         # Provide environment for "nix develop"
         devShells = {
           default = pkgs.mkShell {
-            inherit nativeBuildInputs buildInputs;
+            inherit nativeBuildInputs buildInputs shellHook;
             name = description;
 
             # =========================================================================================
             # Define environment variables
             # =========================================================================================
 
-            # Custom dynamic libraries:
-            # LD_LIBRARY_PATH = builtins.concatStringsSep ":" [
-            #   # Rust Bevy GUI app:
-            #   # "${pkgs.xorg.libX11}/lib"
-            #   # "${pkgs.xorg.libXcursor}/lib"
-            #   # "${pkgs.xorg.libXrandr}/lib"
-            #   # "${pkgs.xorg.libXi}/lib"
-            #   # "${pkgs.libGL}/lib"
-            #
-            #   # JavaFX app:
-            #   # "${pkgs.libGL}/lib"
-            #   # "${pkgs.gtk3}/lib"
-            #   # "${pkgs.glib.out}/lib"
-            #   # "${pkgs.xorg.libXtst}/lib"
-            # ];
-
             # Dynamic libraries from buildinputs:
             LD_LIBRARY_PATH = nixpkgs.lib.makeLibraryPath buildInputs;
-
-            # =========================================================================================
-            # Define shell environment
-            # =========================================================================================
-
-            # Setup the shell when entering the "nix develop" environment (bash script).
-            shellHook = let
-              mkCmakeScript = type: let
-                typeLower = lib.toLower type;
-              in
-                pkgs.writers.writeFish "cmake-${typeLower}.fish" ''
-                  cd $FLAKE_PROJECT_ROOT
-
-                  # set -g -x CC ${pkgs.clang}/bin/clang
-                  # set -g -x CXX ${pkgs.clang}/bin/clang++
-
-                  echo "Removing build directory ./cmake-build-${typeLower}/"
-                  rm -rf ./cmake-build-${typeLower}
-
-                  echo "Creating build directory"
-                  mkdir cmake-build-${typeLower}
-                  cd cmake-build-${typeLower}
-
-                  echo "Running cmake"
-                  cmake -G "Unix Makefiles" \
-                        -DCMAKE_BUILD_TYPE="${type}" \
-                        -DUSE_TRACY=On \
-                        ..
-
-                  echo "Linking compile_commands.json"
-                  cd ..
-                  ln -sf ./cmake-build-${typeLower}/compile_commands.json ./compile_commands.json
-                '';
-
-              cmakeDebug = mkCmakeScript "Debug";
-              cmakeRelease = mkCmakeScript "Release";
-
-              mkBuildScript = type: let
-                typeLower = lib.toLower type;
-              in
-                pkgs.writers.writeFish "cmake-build.fish" ''
-                  cd $FLAKE_PROJECT_ROOT/cmake-build-${typeLower}
-
-                  echo "Running cmake"
-                  NIX_ENFORCE_NO_NATIVE=0 cmake --build . -j$(nproc)
-                '';
-
-              buildDebug = mkBuildScript "Debug";
-              buildRelease = mkBuildScript "Release";
-
-              # Use this to specify commands that should be ran after entering fish shell
-              initProjectShell = pkgs.writers.writeFish "init-shell.fish" ''
-                echo "Entering \"${description}\" environment..."
-
-                # Determine the project root, used e.g. in cmake scripts
-                set -g -x FLAKE_PROJECT_ROOT (git rev-parse --show-toplevel)
-
-                # C/C++:
-                abbr -a cmake-debug "${cmakeDebug}"
-                abbr -a cmake-release "${cmakeRelease}"
-                abbr -a build-debug "${buildDebug}"
-                abbr -a build-release "${buildRelease}"
-                abbr -a debug "${buildDebug} && ./cmake-build-debug/masssprings"
-                abbr -a release "${buildRelease} && ./cmake-build-release/masssprings"
-                abbr -a rungdb "${buildDebug} && gdb --tui ./cmake-build-debug/masssprings"
-                abbr -a runtracy "tracy -a 127.0.0.1 &; ${buildRelease} && sudo -E ./cmake-build-release/masssprings_tracy"
-                abbr -a runvalgrind "${buildDebug} && valgrind --leak-check=full --show-reachable=no --show-leak-kinds=definite,indirect,possible --track-origins=no --suppressions=valgrind.supp --log-file=valgrind.log ./cmake-build-debug/masssprings && cat valgrind.log"
-              '';
-            in
-              builtins.concatStringsSep "\n" [
-                # Launch into pure fish shell
-                ''
-                  exec "$(type -p fish)" -C "source ${initProjectShell} && abbr -a menu '${pkgs.bat}/bin/bat "${initProjectShell}"'"
-                ''
-              ];
           };
 
-          # TODO: Doesn't work
+          # Provide environment with clang stdenv for "nix develop .#clang"
+          # TODO: Broken. Clang can't find stdlib headers or library headers (raylib, backward, ...).
+          #       Does the clangStdenv not automatically collect the include paths?
+          clang =
+            pkgs.mkShell.override {
+              stdenv = pkgs.clangStdenv;
+            } {
+              inherit shellHook;
+              name = description;
 
-          # FHS environment for renderdoc. Access with "nix develop .#renderdoc".
-          # https://ryantm.github.io/nixpkgs/builders/special/fhs-environments
-          # renderdoc =
-          #   (pkgs.buildFHSEnv {
-          #     name = "renderdoc-env";
-          #
-          #     targetPkgs = pkgs:
-          #       with pkgs; [
-          #         # RenderDoc
-          #         renderdoc
-          #
-          #         # Build tools
-          #         gcc
-          #         cmake
-          #
-          #         # Raylib
-          #         raylib
-          #         libGL
-          #         mesa
-          #
-          #         # X11
-          #         libx11
-          #         libxcursor
-          #         libxrandr
-          #         libxinerama
-          #         libxi
-          #         libxext
-          #         libxfixes
-          #
-          #         # Wayland
-          #         wayland
-          #         wayland-protocols
-          #         libxkbcommon
-          #       ];
-          #
-          #     runScript = "fish";
-          #
-          #     profile = ''
-          #     '';
-          #   }).env;
+              nativeBuildInputs = with pkgs; [
+                cmake
+                ninja
+              ];
+
+              buildInputs = with pkgs; [
+                # C/C++:
+                raylib
+                raygui
+                glew
+                thread-pool
+                libmorton
+                boost
+
+                # Debugging/Testing/Profiling
+                backward-cpp
+                libbfd
+                catch2_3
+                gbenchmark
+              ];
+
+              # =========================================================================================
+              # Define environment variables
+              # =========================================================================================
+
+              # Dynamic libraries from buildinputs:
+              LD_LIBRARY_PATH = nixpkgs.lib.makeLibraryPath buildInputs;
+            };
         };
       }
     );

include/bits.hpp

@@ -0,0 +1,69 @@
+#ifndef BITS_HPP_
+#define BITS_HPP_
+
+#include "util.hpp"
+
+#include <concepts>
+
+template <class T>
+    requires std::unsigned_integral<T>
+// ReSharper disable once CppRedundantInlineSpecifier
+INLINE inline auto create_mask(const u8 first, const u8 last) -> T
+{
+    // If the mask width is equal the type width return all 1s instead of shifting
+    // as shifting by type-width is undefined behavior.
+    if (static_cast<size_t>(last - first + 1) >= sizeof(T) * 8) {
+        return ~T{0};
+    }
+
+    // Example: first=4, last=7, 7-4+1=4
+    //           1 << 4 = 0b00010000
+    //          32  - 1 = 0b00001111
+    //          31 << 4 = 0b11110000
+    // Subtracting 1 generates a consecutive mask.
+    return ((T{1} << (last - first + 1)) - 1) << first;
+}
+
+template <class T>
+    requires std::unsigned_integral<T>
+// ReSharper disable once CppRedundantInlineSpecifier
+INLINE inline auto clear_bits(T& bits, const u8 first, const u8 last) -> void
+{
+    const T mask = create_mask<T>(first, last);
+
+    bits = bits & ~mask;
+}
+
+template <class T, class U>
+    requires std::unsigned_integral<T> && std::unsigned_integral<U>
+// ReSharper disable once CppRedundantInlineSpecifier
+INLINE inline auto set_bits(T& bits, const u8 first, const u8 last, const U value) -> void
+{
+    const T mask = create_mask<T>(first, last);
+
+    // Example: first=4, last=6, value=0b1110,   bits = 0b 01111110
+    //          mask                                  = 0b 01110000
+    //          bits & ~mask                          = 0b 00001110
+    //          value << 4                            = 0b 11100000
+    //          (value << 4) & mask                   = 0b 01100000
+    //          (bits & ~mask) | (value << 4) & mask  = 0b 01101110
+    //                                 Insert position:     ^^^
+    // First clear the bits, then | with the value positioned at the insertion point.
+    // The value may be larger than [first, last], extra bits are ignored.
+    bits = (bits & ~mask) | ((static_cast<T>(value) << first) & mask);
+}
+
+template <class T>
+    requires std::unsigned_integral<T>
+// ReSharper disable once CppRedundantInlineSpecifier
+INLINE inline auto get_bits(const T bits, const u8 first, const u8 last) -> T
+{
+    const T mask = create_mask<T>(first, last);
+
+    // We can >> without sign extension because T is unsigned_integral
+    return (bits & mask) >> first;
+}
+
+auto print_bitmap(u64 bitmap, u8 w, u8 h, const std::string& title) -> void;
+
+#endif

include/camera.hpp

@@ -1,49 +0,0 @@
-#ifndef __CAMERA_HPP_
-#define __CAMERA_HPP_
-
-#include "config.hpp"
-
-#include <raylib.h>
-#include <raymath.h>
-
-class OrbitCamera3D {
-  friend class Renderer;
-
-private:
-  Camera camera;
-
-  Vector3 position;
-  Vector3 target;
-  float distance;
-  float angle_x;
-  float angle_y;
-
-  // Input
-  Vector2 last_mouse;
-  bool rotating;
-  bool panning;
-  bool target_lock;
-
-public:
-  OrbitCamera3D()
-      : camera(Camera(Vector3Zero(), Vector3Zero(), Vector3Zero(), 0.0, 0)),
-        position(Vector3Zero()), target(Vector3Zero()),
-        distance(CAMERA_DISTANCE), angle_x(0.0), angle_y(0.0),
-        last_mouse(Vector2Zero()), rotating(false), panning(false),
-        target_lock(true) {
-    camera.position = Vector3(0, 0, -1.0 * distance);
-    camera.target = target;
-    camera.up = Vector3(0, 1.0, 0);
-    camera.fovy = CAMERA_FOV;
-    camera.projection = CAMERA_PERSPECTIVE;
-  }
-
-  ~OrbitCamera3D() {}
-
-public:
-  auto HandleCameraInput() -> Vector2;
-
-  auto Update(const Vector3 &current_target) -> void;
-};
-
-#endif

include/config.hpp

@@ -1,28 +1,57 @@
-#ifndef __CONFIG_HPP_
-#define __CONFIG_HPP_
+#ifndef CONFIG_HPP_
+#define CONFIG_HPP_
 
 #include <raylib.h>
 
-#define THREADPOOL // Enable physics threadpool
-#define BACKWARD   // Enable pretty stack traces
+// Calculate the octree parallel to the layout calculation.
+// Layout uses the octree from last frame.
+#define ASYNC_OCTREE
 
 // Gets set by CMake
+// #define THREADPOOL // Enable physics threadpool
+// #define BACKWARD   // Enable pretty stack traces
 // #define TRACY      // Enable tracy profiling support
 
+#ifdef TRACY
+#include <tracy/Tracy.hpp>
+#endif
+
+#if defined(_WIN32)
+#define NOGDI  // All GDI defines and routines
+#define NOUSER // All USER defines and routines
+#endif
+#define BS_THREAD_POOL_NATIVE_EXTENSIONS
+// ReSharper disable once CppUnusedIncludeDirective
+#include <BS_thread_pool.hpp>
+using threadpool = std::optional<BS::thread_pool<>* const>;
+#if defined(_WIN32) // raylib uses these names as function parameters
+#undef near
+#undef far
+#endif
+
 // Window
-constexpr int INITIAL_WIDTH = 800;
-constexpr int INITIAL_HEIGHT = 800;
-constexpr int MENU_HEIGHT = 250;
+constexpr int INITIAL_WIDTH = 600;
+constexpr int INITIAL_HEIGHT = 600;
+constexpr int MENU_HEIGHT = 300;
+constexpr int POPUP_WIDTH = 450;
+constexpr int POPUP_HEIGHT = 150;
 
 // Menu
 constexpr int MENU_PAD = 5;
 constexpr int BUTTON_PAD = 12;
-constexpr int MENU_ROWS = 5;
+constexpr int MENU_ROWS = 7;
 constexpr int MENU_COLS = 3;
+constexpr const char* FONT = "fonts/SpaceMono.ttf";
+constexpr int FONT_SIZE = 26;
 
 // Camera Controls
-constexpr float CAMERA_FOV = 120.0;
-constexpr float CAMERA_DISTANCE = 20.0;
+constexpr float CAMERA_FOV = 90.0;
+constexpr float FOV_SPEED = 1.0;
+constexpr float FOV_MULTIPLIER = 4.0;
+constexpr float MIN_FOV = 10.0;
+constexpr float MAX_PERSP_FOV = 120.0;
+constexpr float MAX_ORTHO_FOV = 540.0;
+constexpr float CAMERA_DISTANCE = 150.0;
 constexpr float MIN_CAMERA_DISTANCE = 2.0;
 constexpr float MAX_CAMERA_DISTANCE = 2000.0;
 constexpr float ZOOM_SPEED = 2.5;
@@ -37,28 +66,39 @@ constexpr float TARGET_UPS = 90;             // How often to update physics
 constexpr float TIMESTEP = 1.0 / TARGET_UPS; // Update interval in seconds
 constexpr float SIM_SPEED = 4.0;             // How large each update should be
 constexpr float MASS = 1.0;                  // Mass spring system
-constexpr float SPRING_CONSTANT = 5.0;       // Mass spring system
-constexpr float DAMPENING_CONSTANT = 1.0;    // Mass spring system
-constexpr float REST_LENGTH = 2.0;           // Mass spring system
-constexpr float VERLET_DAMPENING = 0.05;     // [0, 1]
-constexpr float BH_FORCE = 2.0;              // Barnes-Hut [1.0, 3.0]
-constexpr float THETA = 0.9;                 // Barnes-Hut [0.5, 1.0]
-constexpr float SOFTENING = 0.01;            // Barnes-Hut [0.01, 1.0]
+constexpr float SPRING_K = 4.0;              // Mass spring system
+constexpr float DAMPENING_K = 1.5;           // Mass spring system
+constexpr float REST_LENGTH = 3.0;           // Mass spring system
+constexpr float VERLET_DAMPENING = 0.1;      // [0, 1]
+constexpr float BH_FORCE = 2.5;              // Barnes-Hut [1.0, 3.0]
+constexpr float THETA = 1.0;                 // Barnes-Hut [0.5, 1.0]
+constexpr float SOFTENING = 0.05;            // Barnes-Hut [0.01, 1.0]
 
 // Graph Drawing
-constexpr float VERTEX_SIZE = 0.5;
-constexpr Color VERTEX_COLOR = GREEN;
-constexpr Color EDGE_COLOR = DARKGREEN;
-constexpr int DRAW_VERTICES_LIMIT = 1000000;
+static const Color EDGE_COLOR = Fade(BLUE, 0.3);
+constexpr int DRAW_EDGES_LIMIT = 5'000'000;
+constexpr float VERTEX_SIZE = 0.75;
+constexpr int DRAW_VERTICES_LIMIT = 1'000'000;
+static const Color VERTEX_COLOR = Fade(BLUE, 0.8);
+constexpr Color VERTEX_VISITED_COLOR = ORANGE;
+constexpr Color VERTEX_START_COLOR = ORANGE;
+constexpr Color VERTEX_CURRENT_COLOR = ORANGE;
+constexpr Color VERTEX_PATH_COLOR = GREEN;
+constexpr Color VERTEX_TARGET_COLOR = GREEN;
+static const Color VERTEX_CLOSEST_COLOR = Fade(PINK, 1.0);
+static const Color VERTEX_FARTHEST_COLOR = Fade(DARKBLUE, 0.8);
 
 // Klotski Drawing
-constexpr int BOARD_PADDING = 5;
-constexpr int BLOCK_PADDING = 5;
-constexpr Color BLOCK_COLOR = DARKGREEN;
-constexpr Color HL_BLOCK_COLOR = GREEN;
+constexpr int BOARD_PADDING = 10;
+constexpr Color BOARD_COLOR_WON = DARKGREEN;
+constexpr Color BOARD_COLOR_RESTRICTED = GRAY;
+constexpr Color BOARD_COLOR_FREE = RAYWHITE;
+constexpr Color BLOCK_COLOR = DARKBLUE;
 constexpr Color TARGET_BLOCK_COLOR = RED;
-constexpr Color HL_TARGET_BLOCK_COLOR = ORANGE;
 constexpr Color WALL_COLOR = BLACK;
-constexpr Color HL_WALL_COLOR = DARKGRAY;
 
-#endif
+// Threadpool
+static constexpr int SMALL_TASK_BLOCK_SIZE = 256; // Weirdly larger blocks decrease performance...
+static constexpr int LARGE_TASK_BLOCK_SIZE = 256;
+
+#endif

include/cpu_layout_engine.hpp

@@ -0,0 +1,100 @@
+#ifndef PHYSICS_HPP_
+#define PHYSICS_HPP_
+
+#include "config.hpp"
+#include "cpu_spring_system.hpp"
+#include "util.hpp"
+
+#include <atomic>
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+#include <raylib.h>
+#include <raymath.h>
+#include <thread>
+#include <variant>
+#include <vector>
+
+class cpu_layout_engine
+{
+    struct add_mass
+    {};
+
+    struct add_spring
+    {
+        size_t a;
+        size_t b;
+    };
+
+    struct clear_graph
+    {};
+
+    using command = std::variant<add_mass, add_spring, clear_graph>;
+
+    struct physics_state
+    {
+#ifdef TRACY
+        TracyLockable(std::mutex, command_mtx);
+#else
+        std::mutex command_mtx;
+#endif
+        std::queue<command> pending_commands;
+
+#ifdef TRACY
+        TracyLockable(std::mutex, data_mtx);
+#else
+        std::mutex data_mtx;
+#endif
+        std::condition_variable_any data_ready_cnd;
+        std::condition_variable_any data_consumed_cnd;
+        Vector3 mass_center = Vector3Zero();
+        int ups = 0;
+        size_t mass_count = 0;       // For debug
+        size_t spring_count = 0;     // For debug
+        std::vector<Vector3> masses; // Read by renderer
+        bool data_ready = false;
+        bool data_consumed = true;
+
+        std::atomic<bool> running{true};
+    };
+
+private:
+    threadpool thread_pool;
+    std::thread physics;
+
+public:
+    physics_state state;
+
+public:
+    explicit cpu_layout_engine(
+        const threadpool _thread_pool = std::nullopt)
+        : thread_pool(_thread_pool), physics(physics_thread, std::ref(state), std::ref(thread_pool))
+    {}
+
+    NO_COPY_NO_MOVE(cpu_layout_engine);
+
+    ~cpu_layout_engine()
+    {
+        state.running = false;
+        state.data_ready_cnd.notify_all();
+        state.data_consumed_cnd.notify_all();
+        physics.join();
+    }
+
+private:
+#ifdef ASYNC_OCTREE
+    static auto set_octree_pool_thread_name(size_t idx) -> void;
+#endif
+
+    static auto physics_thread(physics_state& state,
+                               threadpool thread_pool) -> void;
+
+public:
+    auto clear_cmd() -> void;
+    auto add_mass_cmd() -> void;
+    auto add_spring_cmd(size_t a, size_t b) -> void;
+    auto add_mass_springs_cmd(size_t num_masses,
+                              const std::vector<spring>& springs) -> void;
+};
+
+#endif

include/cpu_spring_system.hpp

@@ -0,0 +1,47 @@
+#ifndef MASS_SPRING_SYSTEM_HPP_
+#define MASS_SPRING_SYSTEM_HPP_
+
+#include "octree.hpp"
+#include "config.hpp"
+
+#include <optional>
+#include <raylib.h>
+
+using spring = std::pair<size_t, size_t>;
+
+class cpu_spring_system
+{
+public:
+    octree tree;
+
+    // This is the main ownership of all the states/masses/springs.
+    std::vector<Vector3> positions;
+    std::vector<Vector3> previous_positions; // for verlet integration
+    std::vector<Vector3> velocities;
+    std::vector<Vector3> forces;
+
+    std::vector<spring> springs;
+
+public:
+    cpu_spring_system() {}
+
+    NO_COPY_NO_MOVE(cpu_spring_system);
+
+public:
+    auto clear() -> void;
+    auto add_mass() -> void;
+    auto add_spring(size_t a, size_t b) -> void;
+
+    auto clear_forces() -> void;
+    auto calculate_spring_force(size_t s) -> void;
+    auto calculate_spring_forces(threadpool thread_pool = std::nullopt) -> void;
+    auto calculate_repulsion_forces(threadpool thread_pool = std::nullopt) -> void;
+    auto integrate_velocity(size_t m, float dt) -> void;
+    auto integrate_position(size_t m, float dt) -> void;
+    auto verlet_update(size_t m, float dt) -> void;
+    auto update(float dt, threadpool thread_pool = std::nullopt) -> void;
+
+    auto center_masses(threadpool thread_pool = std::nullopt) -> void;
+};
+
+#endif

include/distance.hpp

@@ -1,32 +0,0 @@
-#ifndef __DISTANCE_HPP_
-#define __DISTANCE_HPP_
-
-#include "config.hpp"
-
-#include <cstddef>
-#include <vector>
-
-struct DistanceResult {
-  // distances[n] = distance from n to target
-  std::vector<int> distances;
-
-  // parents[n] = next node on the path from n to target
-  std::vector<std::size_t> parents;
-
-  // nearest_target[n] = closest target node to n
-  std::vector<std::size_t> nearest_targets;
-
-  auto Clear() -> void;
-
-  auto Empty() -> bool;
-};
-
-auto CalculateDistances(
-    std::size_t node_count,
-    const std::vector<std::pair<std::size_t, std::size_t>> &edges,
-    const std::vector<std::size_t> &targets) -> DistanceResult;
-
-auto GetPath(const DistanceResult &result, std::size_t source)
-    -> std::vector<std::size_t>;
-
-#endif

include/graph_distances.hpp

@@ -0,0 +1,26 @@
+#ifndef DISTANCE_HPP_
+#define DISTANCE_HPP_
+
+#include "cpu_spring_system.hpp"
+
+#include <vector>
+
+class graph_distances
+{
+public:
+    std::vector<int> distances;          // distances[n] = distance from node n to target
+    std::vector<size_t> parents;         // parents[n] = next node on the path from node n to target
+    std::vector<size_t> nearest_targets; // nearest_target[n] = closest target node to  node n
+
+public:
+    auto clear() -> void;
+    [[nodiscard]] auto empty() const -> bool;
+
+    auto calculate_distances(size_t node_count,
+                             const std::vector<spring>& edges,
+                             const std::vector<size_t>& targets) -> void;
+
+    [[nodiscard]] auto get_shortest_path(size_t source) const -> std::vector<size_t>;
+};
+
+#endif

include/input.hpp

@@ -1,47 +0,0 @@
-#ifndef __INPUT_HPP_
-#define __INPUT_HPP_
-
-#include "config.hpp"
-#include "state.hpp"
-
-class InputHandler {
-public:
-  StateManager &state;
-
-  int hov_x;
-  int hov_y;
-  int sel_x;
-  int sel_y;
-
-  bool has_block_add_xy;
-  int block_add_x;
-  int block_add_y;
-
-  bool mark_path;
-  bool mark_solutions;
-  bool connect_solutions;
-
-public:
-  InputHandler(StateManager &_state)
-      : state(_state), hov_x(-1), hov_y(-1), sel_x(0), sel_y(0),
-        has_block_add_xy(false), block_add_x(-1), block_add_y(-1),
-        mark_path(false), mark_solutions(false), connect_solutions(false) {}
-
-  InputHandler(const InputHandler &copy) = delete;
-  InputHandler &operator=(const InputHandler &copy) = delete;
-  InputHandler(InputHandler &&move) = delete;
-  InputHandler &operator=(InputHandler &&move) = delete;
-
-  ~InputHandler() {}
-
-public:
-  auto HandleMouseHover() -> void;
-
-  auto HandleMouse() -> void;
-
-  auto HandleKeys() -> void;
-
-  auto HandleInput() -> void;
-};
-
-#endif

include/input_handler.hpp

@@ -0,0 +1,176 @@
+#ifndef INPUT_HANDLER_HPP_
+#define INPUT_HANDLER_HPP_
+
+#include "orbit_camera.hpp"
+#include "state_manager.hpp"
+
+#include <functional>
+#include <queue>
+#include <raylib.h>
+#include <raymath.h>
+
+struct show_ok_message
+{
+    std::string title;
+    std::string message;
+};
+
+struct show_yes_no_message
+{
+    std::string title;
+    std::string message;
+    std::function<void()> on_yes;
+};
+
+struct show_save_preset_window {};
+
+using ui_command = std::variant<show_ok_message, show_yes_no_message, show_save_preset_window>;
+
+class input_handler
+{
+    struct generic_handler
+    {
+        std::function<void(input_handler&)> handler;
+    };
+
+    struct mouse_handler : generic_handler
+    {
+        MouseButton button;
+    };
+
+    struct keyboard_handler : generic_handler
+    {
+        KeyboardKey key;
+    };
+
+private:
+    std::vector<generic_handler> generic_handlers;
+    std::vector<mouse_handler> mouse_pressed_handlers;
+    std::vector<mouse_handler> mouse_released_handlers;
+    std::vector<keyboard_handler> key_pressed_handlers;
+    std::vector<keyboard_handler> key_released_handlers;
+
+public:
+    state_manager& state;
+    orbit_camera& camera;
+
+    std::queue<ui_command> ui_commands;
+
+    bool disable = false;
+
+    // Block selection
+    int hov_x = -1;
+    int hov_y = -1;
+    int sel_x = 0;
+    int sel_y = 0;
+
+    // Editing
+    bool editing = false;
+    bool has_block_add_xy = false;
+    int block_add_x = -1;
+    int block_add_y = -1;
+
+    // Graph display
+    bool mark_path = false;
+    bool mark_solutions = false;
+    bool connect_solutions = false;
+    bool color_by_distance = false;
+
+    // Camera
+    bool camera_lock = true;
+    bool camera_mass_center_lock = true;
+    bool camera_panning = false;
+    bool camera_rotating = false;
+
+    // Mouse dragging
+    Vector2 mouse = Vector2Zero();
+    Vector2 last_mouse = Vector2Zero();
+
+    // State selection from graph
+    size_t collision_mass = -1;
+
+public:
+    input_handler(state_manager& _state, orbit_camera& _camera)
+        : state(_state), camera(_camera)
+    {
+        init_handlers();
+    }
+
+    NO_COPY_NO_MOVE(input_handler);
+
+private:
+    auto init_handlers() -> void;
+
+public:
+    // Helpers
+    [[nodiscard]] auto mouse_in_menu_pane() const -> bool;
+    [[nodiscard]] auto mouse_in_board_pane() const -> bool;
+    [[nodiscard]] auto mouse_in_graph_pane() const -> bool;
+
+    // Mouse actions
+    auto mouse_hover() -> void;
+    auto camera_start_pan() -> void;
+    auto camera_pan() const -> void;
+    auto camera_stop_pan() -> void;
+    auto camera_start_rotate() -> void;
+    auto camera_rotate() const -> void;
+    auto camera_stop_rotate() -> void;
+    auto camera_zoom() const -> void;
+    auto camera_fov() const -> void;
+    auto select_block() -> void;
+    auto start_add_block() -> void;
+    auto clear_add_block() -> void;
+    auto add_block() -> void;
+    auto remove_block() -> void;
+    auto place_goal() const -> void;
+    auto select_state() const -> void;
+
+    // Key actions
+    auto toggle_camera_lock() -> void;
+    auto toggle_camera_mass_center_lock() -> void;
+    auto toggle_camera_projection() const -> void;
+    auto move_block_nor() -> void;
+    auto move_block_wes() -> void;
+    auto move_block_sou() -> void;
+    auto move_block_eas() -> void;
+    auto print_state() const -> void;
+    auto load_previous_preset() -> void;
+    auto load_next_preset() -> void;
+    auto goto_starting_state() -> void;
+    auto populate_graph() const -> void;
+    auto clear_graph() -> void;
+    auto toggle_mark_solutions() -> void;
+    auto toggle_connect_solutions() -> void;
+    auto toggle_color_by_distance() -> void;
+    auto toggle_mark_path() -> void;
+    auto goto_optimal_next_state() const -> void;
+    auto goto_most_distant_state() const -> void;
+    auto goto_closest_target_state() const -> void;
+    auto goto_previous_state() const -> void;
+    auto toggle_restricted_movement() const -> void;
+    auto toggle_target_block() const -> void;
+    auto toggle_wall_block() const -> void;
+    auto remove_board_column() const -> void;
+    auto add_board_column() const -> void;
+    auto remove_board_row() const -> void;
+    auto add_board_row() const -> void;
+    auto toggle_editing() -> void;
+    auto clear_goal() const -> void;
+    auto save_preset() -> void;
+
+    // General
+    auto register_generic_handler(const std::function<void(input_handler&)>& handler) -> void;
+
+    auto register_mouse_pressed_handler(MouseButton button, const std::function<void(input_handler&)>& handler) -> void;
+
+    auto register_mouse_released_handler(MouseButton button,
+                                         const std::function<void(input_handler&)>& handler) -> void;
+
+    auto register_key_pressed_handler(KeyboardKey key, const std::function<void(input_handler&)>& handler) -> void;
+
+    auto register_key_released_handler(KeyboardKey key, const std::function<void(input_handler&)>& handler) -> void;
+
+    auto handle_input() -> void;
+};
+
+#endif

include/load_save.hpp

@@ -0,0 +1,15 @@
+#ifndef LOAD_SAVE_HPP_
+#define LOAD_SAVE_HPP_
+
+#include "puzzle.hpp"
+
+#include <string>
+
+auto parse_preset_file(const std::string& preset_file) -> std::pair<std::vector<puzzle>, std::vector<std::string>>;
+auto append_preset_file(const std::string& preset_file, const std::string& preset_name, const puzzle& p) -> bool;
+auto append_preset_file_quiet(const std::string& preset_file,
+                              const std::string& preset_name,
+                              const puzzle& p,
+                              bool validate) -> bool;
+
+#endif

include/octree.hpp

@@ -1,56 +1,210 @@
-#ifndef __OCTREE_HPP_
-#define __OCTREE_HPP_
+#ifndef OCTREE_HPP_
+#define OCTREE_HPP_
 
+#include "util.hpp"
 #include "config.hpp"
 
+#include <array>
+#include <vector>
+
 #include <raylib.h>
 #include <raymath.h>
-#include <vector>
+#include <libmorton/morton.h>
 
-class OctreeNode {
-public:
-  Vector3 mass_center;
-  float mass_total;
-  Vector3 box_min; // area start
-  Vector3 box_max; // area end
-  int children[8];
-  int mass_id;
-  bool leaf;
+class octree
+{
+    class node
+    {
+    public:
+        Vector3 mass_center = Vector3Zero();
+        float mass_total = 0.0;
+        u8 depth = 0;
+        float size = 0.0f; // Because our octree cells are cubic we don't need to store the bounds
+        std::array<int, 8> children = {-1, -1, -1, -1, -1, -1, -1, -1};
+        int mass_id = -1;
+        bool leaf = true;
+    };
+
+private:
+    // 21 * 3 = 63, fits in u64 for combined x/y/z morton-code
+    static constexpr int MAX_DEPTH = 21;
+
+    std::vector<node> nodes;
+
+    // This approach is actually slower than the array of nodes
+    // beacuse we access all the attributes in the same function
+    // std::vector<Vector3> mass_centers;
+    // std::vector<float> mass_totals;
+    // std::vector<Vector3> box_mins;
+    // std::vector<Vector3> box_maxs;
+    // std::vector<std::array<int, 8>> childrens;
+    // std::vector<int> mass_ids;
+    // std::vector<u8> leafs; // bitpacked std::vector<bool> is a lot slower
 
 public:
-  OctreeNode()
-      : mass_center(Vector3Zero()), mass_total(0.0),
-        children(-1, -1, -1, -1, -1, -1, -1, -1), mass_id(-1), leaf(true) {}
+    octree() = default;
+
+    // Required for async octree
+    // NO_COPY_NO_MOVE(octree);
+
+private:
+    [[nodiscard]] INLINE static inline auto get_octant(const Vector3& box_min,
+                                                       const Vector3& box_max,
+                                                       const Vector3& pos) -> int;
+    [[nodiscard]] INLINE static inline auto get_child_bounds(const Vector3& box_min,
+                                                             const Vector3& box_max,
+                                                             int octant) -> std::pair<Vector3, Vector3>;
+
+    // Map a floating point coordinate to a discrete integer (so its morton-code can be computed)
+    // The "bits" parameter determines the discrete axis resolution
+    [[nodiscard]] INLINE static inline auto quantize_axis(float coordinate,
+                                                          float box_min,
+                                                          float box_max,
+                                                          int bits) -> u32;
+
+    [[nodiscard]] INLINE static inline auto pos_to_morton(const Vector3& p,
+                                                          const Vector3& root_min,
+                                                          const Vector3& root_max) -> u64;
+
+    [[nodiscard]] INLINE static inline auto jitter_pos(Vector3 p,
+                                                       u32 seed,
+                                                       const Vector3& root_min,
+                                                       const Vector3& root_max,
+                                                       float root_extent) -> Vector3;
+
+    // Use this to obtain an ancestor node of a leaf node (on any level).
+    // Because the morton codes (interleaved coordinates) encode the octree path, we can take
+    // the morten code of any leaf and only take the 3*n first interleaved bits to find the
+    // leaf ancestor on level n.
+    // Leaf Code: [101 110 100 001] -> Ancestors (from leaf to root):
+    // - [101 110 100]
+    // - [101 110]
+    // - [101] (root)
+    [[nodiscard]] INLINE static inline auto path_to_ancestor(u64 leaf_code, int leaf_depth, int depth) -> u64;
+
+    // Use this to obtain the octant a leaf node is contained in (on any level).
+    // The 3 interleaved bits in the morten code encode the octant [0, 7].
+    // Leaf Code: [101 110 100 001] -> Octants:
+    // - [100] (Level 2)
+    // - [110] (Level 1)
+    // - [101] (Level 0)
+    [[nodiscard]] INLINE static inline auto octant_at_level(u64 leaf_code, int level, int leaf_depth) -> int;
 
 public:
-  auto ChildCount() const -> int;
+    auto clear() -> void;
+    auto reserve(size_t count) -> void;
+    [[nodiscard]] auto empty() const -> bool;
+    [[nodiscard]] auto root() const -> const node&;
+
+    // Morton/linear octree implementation
+    static auto build_octree_morton(octree& t,
+                                    const std::vector<Vector3>& positions,
+                                    const std::optional<BS::thread_pool<>*>& thread_pool) -> void;
+    [[nodiscard]] auto calculate_force_morton(int node_idx, const Vector3& pos, int self_id) const -> Vector3;
 };
 
-class Octree {
-public:
-  std::vector<OctreeNode> nodes;
+INLINE inline auto octree::get_octant(const Vector3& box_min, const Vector3& box_max, const Vector3& pos) -> int
+{
+    auto [cx, cy, cz] = (box_min + box_max) / 2.0f;
 
-public:
-  Octree() {}
+    // The octant is encoded as a 3-bit integer "zyx". The node area is split
+    // along all 3 axes, if a position is right of an axis, this bit is set to 1.
+    // If a position is right of the x-axis and y-axis and left of the z-axis, the
+    // encoded octant is "011".
+    return (pos.x >= cx) | ((pos.y >= cy) << 1) | ((pos.z >= cz) << 2);
+}
 
-  Octree(const Octree &copy) = delete;
-  Octree &operator=(const Octree &copy) = delete;
-  Octree(Octree &&move) = delete;
-  Octree &operator=(Octree &&move) = delete;
+INLINE inline auto octree::get_child_bounds(const Vector3& box_min,
+                                            const Vector3& box_max,
+                                            const int octant) -> std::pair<Vector3, Vector3>
+{
+    auto [cx, cy, cz] = (box_min + box_max) / 2.0f;
 
-public:
-  auto CreateNode(const Vector3 &box_min, const Vector3 &box_max) -> int;
+    Vector3 min = Vector3Zero();
+    Vector3 max = Vector3Zero();
 
-  auto GetOctant(int node_idx, const Vector3 &pos) -> int;
+    // If (octant & 1), the octant is to the right of the node region's x-axis
+    // (see GetOctant). This means the left bound is the x-axis and the right
+    // bound the node's region max.
+    min.x = octant & 1 ? cx : box_min.x;
+    max.x = octant & 1 ? box_max.x : cx;
+    min.y = octant & 2 ? cy : box_min.y;
+    max.y = octant & 2 ? box_max.y : cy;
+    min.z = octant & 4 ? cz : box_min.z;
+    max.z = octant & 4 ? box_max.z : cz;
 
-  auto GetChildBounds(int node_idx, int octant) -> std::pair<Vector3, Vector3>;
+    return std::make_pair(min, max);
+}
 
-  auto Insert(int node_idx, int mass_id, const Vector3 &pos, float mass)
-      -> void;
+INLINE inline auto octree::quantize_axis(const float coordinate,
+                                         const float box_min,
+                                         const float box_max,
+                                         const int bits) -> u32
+{
+    const float extent = box_max - box_min;
+    if (extent <= 0.0f) {
+        return 0;
+    }
 
-  auto CalculateForce(int node_idx, const Vector3 &pos) const -> Vector3;
+    float normalized = (coordinate - box_min) / extent;                            // normalize to [0,1]
+    normalized = std::max(0.0f, std::min(normalized, std::nextafter(1.0f, 0.0f))); // avoid exactly 1.0
 
-  auto Print() const -> void;
-};
+    // bits up to 21 => (1u << bits) safe in 32-bit
+    const u32 grid_max = (1u << bits) - 1u;
+    return static_cast<u32>(normalized * static_cast<float>(grid_max));
+}
 
-#endif
+INLINE inline auto octree::pos_to_morton(const Vector3& p, const Vector3& root_min, const Vector3& root_max) -> u64
+{
+    const u32 x = quantize_axis(p.x, root_min.x, root_max.x, MAX_DEPTH);
+    const u32 y = quantize_axis(p.y, root_min.y, root_max.y, MAX_DEPTH);
+    const u32 z = quantize_axis(p.z, root_min.z, root_max.z, MAX_DEPTH);
+    return libmorton::morton3D_64_encode(x, y, z);
+}
+
+INLINE inline auto octree::jitter_pos(Vector3 p,
+                                      const u32 seed,
+                                      const Vector3& root_min,
+                                      const Vector3& root_max,
+                                      const float root_extent) -> Vector3
+{
+    // Use a hash to calculate a deterministic jitter: The same position should always get the same jitter.
+    // We want this to get stable physics, particles at the same position shouldn't get different jitters
+    // across frames...
+    u32 h = (seed ^ 61u) ^ (seed >> 16);
+    h *= 9u;
+    h = h ^ (h >> 4);
+    h *= 0x27d4eb2du;
+    h = h ^ (h >> 15);
+
+    // finest cell size at depth L
+    const float finest_cell = root_extent / static_cast<float>(1u << MAX_DEPTH);
+    const float s = finest_cell * 1e-4f; // small pp
+
+    p.x += (h & 1u) ? +s : -s;
+    p.y += (h & 2u) ? +s : -s;
+    p.z += (h & 4u) ? +s : -s;
+
+    // clamp back into bounds just in case
+    p.x = std::max(root_min.x, std::min(p.x, root_max.x));
+    p.y = std::max(root_min.y, std::min(p.y, root_max.y));
+    p.z = std::max(root_min.z, std::min(p.z, root_max.z));
+
+    return p;
+}
+
+INLINE inline auto octree::path_to_ancestor(const u64 leaf_code, const int leaf_depth, const int depth) -> u64
+{
+    // keep top 3*depth bits; drop the rest
+    const int drop = 3 * (leaf_depth - depth);
+    return (drop > 0) ? (leaf_code >> drop) : leaf_code;
+}
+
+INLINE inline auto octree::octant_at_level(const u64 leaf_code, const int level, const int leaf_depth) -> int
+{
+    // level 1 => child of root => topmost 3 bits
+    const int shift = 3 * (leaf_depth - level);
+    return static_cast<int>((leaf_code >> shift) & 0x7ull);
+}
+
+#endif

include/orbit_camera.hpp

@@ -0,0 +1,30 @@
+#ifndef CAMERA_HPP_
+#define CAMERA_HPP_
+
+#include "config.hpp"
+
+#include <raylib.h>
+#include <raymath.h>
+
+class orbit_camera
+{
+public:
+    Vector3 position = Vector3Zero();
+    Vector3 target = Vector3Zero();
+    float distance = CAMERA_DISTANCE;
+    float fov = CAMERA_FOV;
+    CameraProjection projection = CAMERA_PERSPECTIVE;
+    float angle_x = 0.0;
+    float angle_y = 0.0;
+
+    Camera camera = Camera{Vector3(0, 0, -distance), target, Vector3(0, 1.0, 0), fov, projection};
+
+public:
+    auto rotate(Vector2 last_mouse, Vector2 mouse) -> void;
+
+    auto pan(Vector2 last_mouse, Vector2 mouse) -> void;
+
+    auto update(const Vector3& current_target, const Vector3& mass_center, bool lock, bool mass_center_lock) -> void;
+};
+
+#endif

include/physics.hpp

@@ -1,192 +0,0 @@
-#ifndef __PHYSICS_HPP_
-#define __PHYSICS_HPP_
-
-#include "config.hpp"
-#include "octree.hpp"
-
-#include <atomic>
-#include <condition_variable>
-#include <cstddef>
-#include <iostream>
-#include <mutex>
-#include <queue>
-#include <raylib.h>
-#include <raymath.h>
-#include <thread>
-#include <variant>
-#include <vector>
-
-#ifdef THREADPOOL
-#define BS_THREAD_POOL_NATIVE_EXTENSIONS
-#include <BS_thread_pool.hpp>
-#endif
-
-#ifdef TRACY
-#include <tracy/Tracy.hpp>
-#endif
-
-class Mass {
-public:
-  Vector3 position;
-  Vector3 previous_position; // for verlet integration
-  Vector3 velocity;
-  Vector3 force;
-
-public:
-  Mass(Vector3 _position)
-      : position(_position), previous_position(_position),
-        velocity(Vector3Zero()), force(Vector3Zero()) {}
-
-public:
-  auto ClearForce() -> void;
-
-  auto CalculateVelocity(const float delta_time) -> void;
-
-  auto CalculatePosition(const float delta_time) -> void;
-
-  auto VerletUpdate(const float delta_time) -> void;
-};
-
-class Spring {
-public:
-  std::size_t a;
-  std::size_t b;
-
-public:
-  Spring(std::size_t _a, std::size_t _b) : a(_a), b(_b) {}
-
-public:
-  auto CalculateSpringForce(Mass &_a, Mass &_b) const -> void;
-};
-
-class MassSpringSystem {
-private:
-  Octree octree;
-
-#ifdef THREADPOOL
-  BS::thread_pool<BS::tp::none> threads;
-#endif
-
-public:
-  // This is the main ownership of all the states/masses/springs.
-  std::vector<Mass> masses;
-  std::vector<Spring> springs;
-
-public:
-  MassSpringSystem()
-#ifdef THREADPOOL
-      : threads(std::thread::hardware_concurrency() - 1, SetThreadName)
-#endif
-  {
-    std::cout << "Using Barnes-Hut + octree repulsion force calculation."
-              << std::endl;
-
-#ifdef THREADPOOL
-    std::cout << "Thread-Pool: " << threads.get_thread_count() << " threads."
-              << std::endl;
-#endif
-  };
-
-  MassSpringSystem(const MassSpringSystem &copy) = delete;
-  MassSpringSystem &operator=(const MassSpringSystem &copy) = delete;
-  MassSpringSystem(MassSpringSystem &move) = delete;
-  MassSpringSystem &operator=(MassSpringSystem &&move) = delete;
-
-private:
-#ifdef THREADPOOL
-  static auto SetThreadName(std::size_t idx) -> void;
-#endif
-
-  auto BuildOctree() -> void;
-
-public:
-  auto AddMass() -> void;
-
-  auto AddSpring(int a, int b) -> void;
-
-  auto Clear() -> void;
-
-  auto ClearForces() -> void;
-
-  auto CalculateSpringForces() -> void;
-
-  auto CalculateRepulsionForces() -> void;
-
-  auto VerletUpdate(float delta_time) -> void;
-};
-
-class ThreadedPhysics {
-  struct AddMass {};
-  struct AddSpring {
-    std::size_t a;
-    std::size_t b;
-  };
-  struct ClearGraph {};
-
-  using Command = std::variant<AddMass, AddSpring, ClearGraph>;
-
-  struct PhysicsState {
-#ifdef TRACY
-    TracyLockable(std::mutex, command_mtx);
-#else
-    std::mutex command_mtx;
-#endif
-    std::queue<Command> pending_commands;
-
-#ifdef TRACY
-    TracyLockable(std::mutex, data_mtx);
-#else
-    std::mutex data_mtx;
-#endif
-    std::condition_variable_any data_ready_cnd;
-    std::condition_variable_any data_consumed_cnd;
-    unsigned int ups = 0;
-    std::vector<Vector3> masses; // Read by renderer
-    bool data_ready = false;
-    bool data_consumed = true;
-
-    std::atomic<bool> running{true};
-  };
-
-private:
-  std::thread physics;
-
-public:
-  PhysicsState state;
-
-public:
-  ThreadedPhysics() : physics(PhysicsThread, std::ref(state)) {}
-
-  ThreadedPhysics(const ThreadedPhysics &copy) = delete;
-  ThreadedPhysics &operator=(const ThreadedPhysics &copy) = delete;
-  ThreadedPhysics(ThreadedPhysics &&move) = delete;
-  ThreadedPhysics &operator=(ThreadedPhysics &&move) = delete;
-
-  ~ThreadedPhysics() {
-    state.running = false;
-    state.data_ready_cnd.notify_all();
-    state.data_consumed_cnd.notify_all();
-    physics.join();
-  }
-
-private:
-  static auto PhysicsThread(PhysicsState &state) -> void;
-
-public:
-  auto AddMassCmd() -> void;
-
-  auto AddSpringCmd(std::size_t a, std::size_t b) -> void;
-
-  auto ClearCmd() -> void;
-
-  auto AddMassSpringsCmd(
-      std::size_t num_masses,
-      const std::vector<std::pair<std::size_t, std::size_t>> &springs) -> void;
-};
-
-// https://en.cppreference.com/w/cpp/utility/variant/visit
-template <class... Ts> struct overloads : Ts... {
-  using Ts::operator()...;
-};
-
-#endif

include/renderer.hpp

@@ -1,79 +1,130 @@
-#ifndef __RENDERER_HPP_
-#define __RENDERER_HPP_
+#ifndef RENDERER_HPP_
+#define RENDERER_HPP_
 
-#include "camera.hpp"
 #include "config.hpp"
-#include "input.hpp"
-#include "state.hpp"
+#include "orbit_camera.hpp"
+#include "input_handler.hpp"
+#include "state_manager.hpp"
+#include "user_interface.hpp"
 
 #include <raylib.h>
-#include <raymath.h>
+#include <rlgl.h>
 
-class Renderer {
+class renderer
+{
 private:
-  const StateManager &state;
-  const InputHandler &input;
+    const state_manager& state;
+    input_handler& input;
+    user_interface& gui;
 
-  const OrbitCamera3D &camera;
-  RenderTexture render_target;
-  RenderTexture klotski_target;
-  RenderTexture menu_target;
+    const orbit_camera& camera;
+    RenderTexture graph_target = LoadRenderTexture(GetScreenWidth() / 2, GetScreenHeight() - MENU_HEIGHT);
 
-  // Instancing
-  Material vertex_mat;
-  std::size_t transforms_size;
-  Matrix *transforms;
-  Mesh cube_instance;
-  Shader instancing_shader;
+    // TODO: Those should be moved to the user_interface.h
+    RenderTexture klotski_target = LoadRenderTexture(GetScreenWidth() / 2, GetScreenHeight() - MENU_HEIGHT);
+    RenderTexture menu_target = LoadRenderTexture(GetScreenWidth(), MENU_HEIGHT);
+
+    // Edges
+    unsigned int edge_vao_id = 0;
+    unsigned int edge_vbo_id = 0;
+    std::vector<Vector3> edge_vertices;
+    Shader edge_shader = LoadShader("shader/edge_vertex.glsl", "shader/edge_fragment.glsl");
+    int edge_color_loc = -1;
+    std::vector<std::pair<Vector3, Vector3>> connections;
+
+    // Vertex instancing
+    static constexpr int INSTANCE_COLOR_ATTR = 5;
+    std::vector<Matrix> transforms;
+    std::vector<Color> colors;
+    Material vertex_mat = LoadMaterialDefault();
+    Mesh cube_instance = GenMeshCube(VERTEX_SIZE, VERTEX_SIZE, VERTEX_SIZE);
+    Shader instancing_shader = LoadShader("shader/instancing_vertex.glsl", "shader/instancing_fragment.glsl");
+    unsigned int color_vbo_id = 0;
 
 public:
-  Renderer(const OrbitCamera3D &_camera, const StateManager &_state,
-           const InputHandler &_input)
-      : state(_state), input(_input), camera(_camera), transforms_size(0),
-        transforms(nullptr) {
-    render_target = LoadRenderTexture(GetScreenWidth() / 2.0,
-                                      GetScreenHeight() - MENU_HEIGHT);
-    klotski_target = LoadRenderTexture(GetScreenWidth() / 2.0,
-                                       GetScreenHeight() - MENU_HEIGHT);
-    menu_target = LoadRenderTexture(GetScreenWidth(), MENU_HEIGHT);
-  }
+    // TODO: I am allocating HUGE vertex buffers instead of resizing dynamically...
+    //       Edges: 5'000'000 * 2 * 12 Byte ~= 115 MB
+    //       Verts: 1'000'000 * 16 Byte ~= 15 MB
+    //       This is also allocated on the CPU by the vectors
+    renderer(const orbit_camera& _camera, const state_manager& _state, input_handler& _input, user_interface& _gui)
+        : state(_state), input(_input), gui(_gui), camera(_camera)
+    {
+        // Edges
+        edge_shader.locs[SHADER_LOC_VERTEX_POSITION] = GetShaderLocationAttrib(edge_shader, "vertexPosition");
+        edge_shader.locs[SHADER_LOC_MATRIX_MVP] = GetShaderLocation(edge_shader, "mvp");
+        edge_shader.locs[SHADER_LOC_COLOR_DIFFUSE] = GetShaderLocation(edge_shader, "colDiffuse");
+        edge_color_loc = GetShaderLocation(edge_shader, "colDiffuse");
 
-  Renderer(const Renderer &copy) = delete;
-  Renderer &operator=(const Renderer &copy) = delete;
-  Renderer(Renderer &&move) = delete;
-  Renderer &operator=(Renderer &&move) = delete;
+        edge_vertices.reserve(DRAW_EDGES_LIMIT * 2);
 
-  ~Renderer() {
-    UnloadRenderTexture(render_target);
-    UnloadRenderTexture(klotski_target);
-    UnloadRenderTexture(menu_target);
+        edge_vao_id = rlLoadVertexArray();
+        edge_vbo_id = rlLoadVertexBuffer(nullptr, DRAW_EDGES_LIMIT * 2 * sizeof(Vector3), true);
 
-    // Instancing
-    if (transforms != nullptr) {
-      UnloadMaterial(vertex_mat);
-      MemFree(transforms);
-      UnloadMesh(cube_instance);
+        rlEnableVertexArray(edge_vao_id);
+        rlEnableVertexBuffer(edge_vbo_id);
 
-      // I think the shader already gets unloaded with the material?
-      // UnloadShader(instancing_shader);
+        rlSetVertexAttribute(0, 3, RL_FLOAT, false, sizeof(Vector3), 0);
+        rlEnableVertexAttribute(0);
+
+        rlDisableVertexBuffer();
+        rlDisableVertexArray();
+
+        // Vertex instancing
+        instancing_shader.locs[SHADER_LOC_MATRIX_MVP] = GetShaderLocation(instancing_shader, "mvp");
+        instancing_shader.locs[SHADER_LOC_MATRIX_MODEL] = GetShaderLocationAttrib(
+            instancing_shader,
+            "instanceTransform");
+        instancing_shader.locs[SHADER_LOC_VECTOR_VIEW] = GetShaderLocation(instancing_shader, "viewPos");
+
+        vertex_mat.shader = instancing_shader;
+
+        transforms.reserve(DRAW_VERTICES_LIMIT);
+        colors.reserve(DRAW_VERTICES_LIMIT);
+        color_vbo_id = rlLoadVertexBuffer(nullptr, DRAW_VERTICES_LIMIT * sizeof(Color), true);
+
+        rlEnableVertexArray(cube_instance.vaoId);
+        rlEnableVertexBuffer(color_vbo_id);
+        rlSetVertexAttribute(INSTANCE_COLOR_ATTR, 4, RL_UNSIGNED_BYTE, true, 0, 0);
+        rlEnableVertexAttribute(INSTANCE_COLOR_ATTR);
+        rlSetVertexAttributeDivisor(INSTANCE_COLOR_ATTR, 1);
+
+        rlDisableVertexBuffer();
+        rlDisableVertexArray();
+    }
+
+    NO_COPY_NO_MOVE(renderer);
+
+    ~renderer()
+    {
+        UnloadRenderTexture(graph_target);
+        UnloadRenderTexture(klotski_target);
+        UnloadRenderTexture(menu_target);
+
+        // Edges
+        rlUnloadVertexArray(edge_vao_id);
+        rlUnloadVertexBuffer(edge_vbo_id);
+        UnloadShader(edge_shader);
+
+        // Instancing
+        UnloadMaterial(vertex_mat);
+        UnloadMesh(cube_instance);
+
+        // I think the shader already gets unloaded with the material?
+        // UnloadShader(instancing_shader);
+
+        rlUnloadVertexBuffer(color_vbo_id);
     }
-  }
 
 private:
-  auto AllocateGraphInstancing(std::size_t size) -> void;
+    auto update_texture_sizes() -> void;
 
-  auto ReallocateGraphInstancingIfNecessary(std::size_t size) -> void;
+    auto draw_mass_springs(const std::vector<Vector3>& masses) -> void;
+    auto draw_klotski() const -> void;
+    auto draw_menu() const -> void;
+    auto draw_textures(int fps, int ups, size_t mass_count, size_t spring_count) const -> void;
 
 public:
-  auto UpdateTextureSizes() -> void;
-
-  auto DrawMassSprings(const std::vector<Vector3> &masses) -> void;
-
-  auto DrawKlotski() -> void;
-
-  auto DrawMenu(const std::vector<Vector3> &masses) -> void;
-
-  auto DrawTextures(float ups) -> void;
+    auto render(const std::vector<Vector3>& masses, int fps, int ups, size_t mass_count, size_t spring_count) -> void;
 };
 
-#endif
+#endif

include/state.hpp

@@ -1,99 +0,0 @@
-#ifndef __STATE_HPP_
-#define __STATE_HPP_
-
-#include "config.hpp"
-#include "distance.hpp"
-#include "physics.hpp"
-#include "puzzle.hpp"
-
-#include <raymath.h>
-#include <stack>
-#include <unordered_map>
-#include <unordered_set>
-
-class StateManager {
-public:
-  ThreadedPhysics &physics;
-
-  std::vector<State> presets;
-  std::vector<std::string> comments;
-
-  // Some stuff is faster to map from state to mass (e.g. in the renderer)
-  std::unordered_map<State, std::size_t> states;
-  std::unordered_set<State> winning_states;
-  std::unordered_set<State> visited_states;
-  std::stack<State> history;
-
-  // Other stuff maps from mass to state :/
-  std::unordered_map<std::size_t, State> masses;
-  std::vector<std::size_t> winning_path;
-
-  // Fuck it, duplicate the springs too, we don't even need to copy them from
-  // the physics thread then...
-  std::vector<std::pair<std::size_t, std::size_t>> springs;
-
-  // Distance calculation result can be buffered and reused to calculate a new
-  // path on the same graph
-  DistanceResult target_distances;
-
-  int current_preset;
-  State starting_state;
-  State current_state;
-  State previous_state;
-
-  bool edited = false;
-
-public:
-  StateManager(ThreadedPhysics &_physics, const std::string &preset_file)
-      : physics(_physics), presets({State()}), current_preset(0),
-        edited(false) {
-    ParsePresetFile(preset_file);
-    current_state = presets.at(current_preset);
-    ClearGraph();
-  }
-
-  StateManager(const StateManager &copy) = delete;
-  StateManager &operator=(const StateManager &copy) = delete;
-  StateManager(StateManager &&move) = delete;
-  StateManager &operator=(StateManager &&move) = delete;
-
-  ~StateManager() {}
-
-private:
-  auto ParsePresetFile(const std::string &preset_file) -> void;
-
-public:
-  auto LoadPreset(int preset) -> void;
-
-  auto ResetState() -> void;
-
-  auto PreviousPreset() -> void;
-
-  auto NextPreset() -> void;
-
-  auto NextPath() -> void;
-
-  auto FillGraph() -> void;
-
-  auto UpdateGraph() -> void;
-
-  auto ClearGraph() -> void;
-
-  auto FindWinningStates() -> void;
-
-  auto FindTargetDistances() -> void;
-
-  auto FindTargetPath() -> void;
-
-  auto FindWorstState() -> State;
-
-  auto GoToWorst() -> void;
-
-  auto GoToNearestTarget() -> void;
-
-  auto PopHistory() -> void;
-
-  auto CurrentMassIndex() const -> std::size_t;
-};
-
-#endif

include/state_manager.hpp

@@ -0,0 +1,145 @@
+#ifndef STATE_MANAGER_HPP_
+#define STATE_MANAGER_HPP_
+
+#include "graph_distances.hpp"
+#include "load_save.hpp"
+#include "cpu_layout_engine.hpp"
+#include "puzzle.hpp"
+
+#include <boost/unordered/unordered_flat_map.hpp>
+#include <boost/unordered/unordered_flat_set.hpp>
+
+class state_manager
+{
+private:
+    cpu_layout_engine& physics;
+
+    std::string preset_file;
+    size_t current_preset = 0;
+    std::vector<puzzle> preset_states = {puzzle(4, 5, 0, 0, true, false)};
+    std::vector<std::string> preset_comments = {"# Empty"};
+
+    // State storage (store states twice for bidirectional lookup).
+    // Everything else should only store indices to state_pool.
+
+    std::vector<puzzle> state_pool;  // Indices are equal to mass_springs mass indices
+    puzzlemap<size_t> state_indices; // Maps states to indices
+    std::vector<spring> links;       // Indices are equal to mass_springs springs indices
+
+    graph_distances node_target_distances; // Buffered and reused if the graph doesn't change
+    boost::unordered_flat_set<size_t> winning_indices; // Indices of all states where the board is solved
+    std::vector<size_t> winning_path; // Ordered list of node indices leading to the nearest solved state
+    boost::unordered_flat_set<size_t> path_indices; // For faster lookup if a vertex is part of the path in renderer
+
+    std::vector<size_t> move_history;                    // Moves between the starting state and the current state
+    boost::unordered_flat_map<size_t, int> visit_counts; // How often each state was visited
+
+    size_t starting_state_index = 0;
+    size_t current_state_index = 0;
+    size_t previous_state_index = 0;
+
+    int total_moves = 0;
+    bool edited = false;
+
+public:
+    state_manager(cpu_layout_engine& _physics, const std::string& _preset_file)
+        : physics(_physics), preset_file(_preset_file)
+    {
+        reload_preset_file();
+    }
+
+    NO_COPY_NO_MOVE(state_manager);
+
+private:
+    /**
+     * Inserts a board state into the state_manager and the physics system.
+     * States should only be inserted using this function to keep both systems in sync.
+     * The function checks for duplicates before insertion.
+     *
+     * @param state State to insert
+     * @return Index of insertion (or existing index if duplicate)
+     */
+    auto synced_try_insert_state(const puzzle& state) -> size_t;
+
+    /**
+     * Inserts a state link into the state_manager and the physics system.
+     * Links should only be inserted using this function to keep both systems in sync.
+     * The function does not check for duplicates before insertion.
+     *
+     * @param first_index Index of the first linked state
+     * @param second_index Index of the second linked state
+     */
+    auto synced_insert_link(size_t first_index, size_t second_index) -> void;
+
+    /**
+     * Inserts an entire statespace into the state_manager and the physics system.
+     * If inserting many states and links in bulk, this function should always be used
+     * to not stress the physics command mutex.
+     * The function does not check for duplicates before insertion.
+     *
+     * @param states List of states to insert
+     * @param _links List of links to insert
+     */
+    auto synced_insert_statespace(const std::vector<puzzle>& states, const std::vector<spring>& _links) -> void;
+
+    /**
+     * Clears all states and links (and related) from the state_manager and the physics system.
+     * Note that this leaves any dangling indices (e.g., current_state_index) in an invalid state.
+     */
+    auto synced_clear_statespace() -> void;
+
+public:
+    // Presets
+    auto save_current_to_preset_file(const std::string& preset_comment) -> void;
+    auto reload_preset_file() -> void;
+    auto load_preset(size_t preset) -> void;
+    auto load_previous_preset() -> void;
+    auto load_next_preset() -> void;
+
+    // Update current_state
+    auto update_current_state(const puzzle& p) -> void;
+    auto edit_starting_state(const puzzle& p) -> void;
+    auto goto_starting_state() -> void;
+    auto goto_optimal_next_state() -> void;
+    auto goto_previous_state() -> void;
+    auto goto_most_distant_state() -> void;
+    auto goto_closest_target_state() -> void;
+
+    // Update graph
+    auto populate_graph() -> void;
+    auto clear_graph_and_add_current(const puzzle& p) -> void;
+    auto clear_graph_and_add_current() -> void;
+    auto populate_winning_indices() -> void;
+    auto populate_node_target_distances() -> void;
+    auto populate_winning_path() -> void;
+
+    // Index mapping
+    [[nodiscard]] auto get_index(const puzzle& state) const -> size_t;
+    [[nodiscard]] auto get_current_index() const -> size_t;
+    [[nodiscard]] auto get_starting_index() const -> size_t;
+    [[nodiscard]] auto get_state(size_t index) const -> const puzzle&;
+    [[nodiscard]] auto get_current_state() const -> const puzzle&;
+    [[nodiscard]] auto get_starting_state() const -> const puzzle&;
+
+    // Access
+    [[nodiscard]] auto get_state_count() const -> size_t;
+    [[nodiscard]] auto get_target_count() const -> size_t;
+    [[nodiscard]] auto get_link_count() const -> size_t;
+    [[nodiscard]] auto get_path_length() const -> size_t;
+    [[nodiscard]] auto get_links() const -> const std::vector<spring>&;
+    [[nodiscard]] auto get_winning_indices() const -> const boost::unordered_flat_set<size_t>&;
+    [[nodiscard]] auto get_visit_counts() const -> const boost::unordered_flat_map<size_t, int>&;
+    [[nodiscard]] auto get_winning_path() const -> const std::vector<size_t>&;
+    [[nodiscard]] auto get_path_indices() const -> const boost::unordered_flat_set<size_t>&;
+    [[nodiscard]] auto get_current_visits() const -> int;
+    [[nodiscard]] auto get_current_preset() const -> size_t;
+    [[nodiscard]] auto get_preset_count() const -> size_t;
+    [[nodiscard]] auto get_current_preset_comment() const -> const std::string&;
+    [[nodiscard]] auto has_history() const -> bool;
+    [[nodiscard]] auto has_distances() const -> bool;
+    [[nodiscard]] auto get_distances() const -> std::vector<int>;
+    [[nodiscard]] auto get_total_moves() const -> size_t;
+    [[nodiscard]] auto was_edited() const -> bool;
+};
+
+#endif

include/tracy.hpp

@@ -1,16 +0,0 @@
-#ifndef __TRACY_HPP_
-#define __TRACY_HPP_
-
-#include "config.hpp"
-
-#ifdef TRACY
-
-#include <cstddef>
-
-void *operator new(std::size_t count);
-void operator delete(void *ptr) noexcept;
-void operator delete(void *ptr, std::size_t count) noexcept;
-
-#endif
-
-#endif

include/user_interface.hpp

@@ -0,0 +1,223 @@
+#ifndef USER_INTERFACE_HPP_
+#define USER_INTERFACE_HPP_
+
+#include "orbit_camera.hpp"
+#include "config.hpp"
+#include "input_handler.hpp"
+#include "state_manager.hpp"
+
+#include <raylib.h>
+
+class user_interface
+{
+    class grid
+    {
+    public:
+        int x;
+        int y;
+        int width;
+        int height;
+        int columns;
+        int rows;
+        const int padding;
+
+    public:
+        grid(const int _x,
+             const int _y,
+             const int _width,
+             const int _height,
+             const int _columns,
+             const int _rows,
+             const int _padding)
+            : x(_x), y(_y), width(_width), height(_height), columns(_columns), rows(_rows), padding(_padding) {}
+
+    public:
+        auto update_bounds(int _x, int _y, int _width, int _height, int _columns, int _rows) -> void;
+        auto update_bounds(int _x, int _y, int _width, int _height) -> void;
+        auto update_bounds(int _x, int _y) -> void;
+
+        [[nodiscard]] auto bounds() const -> Rectangle;
+        [[nodiscard]] auto bounds(int _x, int _y, int _width, int _height) const -> Rectangle;
+
+        [[nodiscard]] auto square_bounds() const -> Rectangle;
+        [[nodiscard]] auto square_bounds(int _x, int _y, int _width, int _height) const -> Rectangle;
+    };
+
+    struct style
+    {
+        int border_color_normal;
+        int base_color_normal;
+        int text_color_normal;
+
+        int border_color_focused;
+        int base_color_focused;
+        int text_color_focused;
+
+        int border_color_pressed;
+        int base_color_pressed;
+        int text_color_pressed;
+
+        int border_color_disabled;
+        int base_color_disabled;
+        int text_color_disabled;
+    };
+
+    struct default_style : style
+    {
+        int background_color;
+        int line_color;
+
+        int text_size;
+        int text_spacing;
+        int text_line_spacing;
+        int text_alignment_vertical;
+        int text_wrap_mode;
+    };
+
+    struct component_style : style
+    {
+        int border_width;
+        int text_padding;
+        int text_alignment;
+    };
+
+private:
+    input_handler& input;
+    state_manager& state;
+    const orbit_camera& camera;
+
+    grid menu_grid = grid(0, 0, GetScreenWidth(), MENU_HEIGHT, MENU_COLS, MENU_ROWS, MENU_PAD);
+
+    grid board_grid = grid(0,
+                           MENU_HEIGHT,
+                           GetScreenWidth() / 2,
+                           GetScreenHeight() - MENU_HEIGHT,
+                           state.get_current_state().get_width(),
+                           state.get_current_state().get_height(),
+                           BOARD_PADDING);
+
+    grid graph_overlay_grid = grid(GetScreenWidth() / 2, MENU_HEIGHT, 200, 100, 1, 4, MENU_PAD);
+
+    grid debug_overlay_grid = grid(GetScreenWidth() / 2, GetScreenHeight() - 75, 200, 75, 1, 3, MENU_PAD);
+
+    // Windows
+
+    std::string message_title;
+    std::string message_message;
+    std::function<void()> yes_no_handler;
+    bool ok_message = false;
+    bool yes_no_message = false;
+    bool save_window = false;
+    std::array<char, 256> preset_comment = {};
+    bool help_window = false;
+
+public:
+    user_interface(input_handler& _input, state_manager& _state, const orbit_camera& _camera)
+        : input(_input), state(_state), camera(_camera)
+    {
+        init();
+    }
+
+    NO_COPY_NO_MOVE(user_interface);
+
+private:
+    static auto init() -> void;
+
+    static auto apply_color(style& style, Color color) -> void;
+    static auto apply_block_color(style& style, Color color) -> void;
+    static auto apply_text_color(style& style, Color color) -> void;
+
+    static auto get_default_style() -> default_style;
+    static auto set_default_style(const default_style& style) -> void;
+    static auto get_component_style(int component) -> component_style;
+    static auto set_component_style(int component, const component_style& style) -> void;
+
+    [[nodiscard]] static auto popup_bounds() -> Rectangle;
+
+    auto draw_button(Rectangle bounds,
+                     const std::string& label,
+                     Color color,
+                     bool enabled = true,
+                     int font_size = FONT_SIZE) const -> int;
+
+    auto draw_menu_button(int x,
+                          int y,
+                          int width,
+                          int height,
+                          const std::string& label,
+                          Color color,
+                          bool enabled = true,
+                          int font_size = FONT_SIZE) const -> int;
+
+    auto draw_toggle_slider(Rectangle bounds,
+                            const std::string& off_label,
+                            const std::string& on_label,
+                            int* active,
+                            Color color,
+                            bool enabled = true,
+                            int font_size = FONT_SIZE) const -> int;
+
+    auto draw_menu_toggle_slider(int x,
+                                 int y,
+                                 int width,
+                                 int height,
+                                 const std::string& off_label,
+                                 const std::string& on_label,
+                                 int* active,
+                                 Color color,
+                                 bool enabled = true,
+                                 int font_size = FONT_SIZE) const -> int;
+
+    auto draw_spinner(Rectangle bounds,
+                      const std::string& label,
+                      int* value,
+                      int min,
+                      int max,
+                      Color color,
+                      bool enabled = true,
+                      int font_size = FONT_SIZE) const -> int;
+
+    auto draw_menu_spinner(int x,
+                           int y,
+                           int width,
+                           int height,
+                           const std::string& label,
+                           int* value,
+                           int min,
+                           int max,
+                           Color color,
+                           bool enabled = true,
+                           int font_size = FONT_SIZE) const -> int;
+
+    auto draw_label(Rectangle bounds,
+                    const std::string& text,
+                    Color color,
+                    bool enabled = true,
+                    int font_size = FONT_SIZE) const -> int;
+
+    auto draw_board_block(int x, int y, int width, int height, Color color, bool enabled = true) const -> bool;
+
+    [[nodiscard]] auto window_open() const -> bool;
+
+    // Different menu sections
+    auto draw_menu_header(Color color) const -> void;
+    auto draw_graph_info(Color color) const -> void;
+    auto draw_graph_controls(Color color) const -> void;
+    auto draw_camera_controls(Color color) const -> void;
+    auto draw_puzzle_controls(Color color) const -> void;
+    auto draw_edit_controls(Color color) const -> void;
+    auto draw_menu_footer(Color color) -> void;
+
+public:
+    static auto get_background_color() -> Color;
+    auto help_popup() -> void;
+    auto draw_save_preset_popup() -> void;
+    auto draw_ok_message_box() -> void;
+    auto draw_yes_no_message_box() -> void;
+    auto draw_main_menu() -> void;
+    auto draw_puzzle_board() -> void;
+    auto draw_graph_overlay(int fps, int ups, size_t mass_count, size_t spring_count) -> void;
+    auto draw(int fps, int ups, size_t mass_count, size_t spring_count) -> void;
+};
+
+#endif

include/util.hpp

@@ -1,20 +1,156 @@
-#ifndef __UTIL_HPP_
-#define __UTIL_HPP_
-
-#include "config.hpp"
+#ifndef UTIL_HPP_
+#define UTIL_HPP_
 
+#include <vector>
 #include <iostream>
 #include <raylib.h>
-#include <raymath.h>
 
-inline std::ostream &operator<<(std::ostream &os, const Vector2 &v) {
-  os << "(" << v.x << ", " << v.y << ")";
-  return os;
+#define INLINE __attribute__((always_inline))
+#define PACKED __attribute__((packed))
+
+#define STARTTIME const auto start = std::chrono::high_resolution_clock::now()
+#define ENDTIME(msg, cast, unit) const auto end = std::chrono::high_resolution_clock::now(); \
+    infoln("{}. Took {}{}.", msg, std::chrono::duration_cast<cast>(end - start).count(), unit)
+
+#define COMMENT if (false)
+
+#define NO_COPY_NO_MOVE(typename) \
+    typename(const typename& copy) = delete; \
+    auto operator=(const typename& copy) -> typename& = delete; \
+    typename(typename&& move) = delete; \
+    auto operator=(typename&& move) -> typename& = delete;
+
+using u8 = uint8_t;
+using u16 = uint16_t;
+using u32 = uint32_t;
+using u64 = uint64_t;
+
+using i8 = int8_t;
+using i16 = int16_t;
+using i32 = int32_t;
+using i64 = int64_t;
+
+// https://en.cppreference.com/w/cpp/utility/variant/visit
+template <class... Ts>
+struct overloads : Ts...
+{
+    using Ts::operator()...;
+};
+
+inline auto binom(const int n, const int k) -> int
+{
+    std::vector<int> solutions(k);
+    solutions[0] = n - k + 1;
+
+    for (int i = 1; i < k; ++i) {
+        solutions[i] = solutions[i - 1] * (n - k + 1 + i) / (i + 1);
+    }
+
+    return solutions[k - 1];
 }
 
-inline std::ostream &operator<<(std::ostream &os, const Vector3 &v) {
-  os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
-  return os;
+// Enums
+
+enum dir : u8
+{
+    nor = 1 << 0,
+    eas = 1 << 1,
+    sou = 1 << 2,
+    wes = 1 << 3,
+};
+
+// Ansi
+
+enum class ctrl : u8
+{
+    reset = 0,
+    bold_bright = 1,
+    underline = 4,
+    inverse = 7,
+    bold_bright_off = 21,
+    underline_off = 24,
+    inverse_off = 27
+};
+
+enum class fg : u8
+{
+    black = 30,
+    red = 31,
+    green = 32,
+    yellow = 33,
+    blue = 34,
+    magenta = 35,
+    cyan = 36,
+    white = 37
+};
+
+enum class bg : u8
+{
+    black = 40,
+    red = 41,
+    green = 42,
+    yellow = 43,
+    blue = 44,
+    magenta = 45,
+    cyan = 46,
+    white = 47
+};
+
+inline auto ansi_bold_fg(const fg color) -> std::string
+{
+    return std::format("\033[{};{}m", static_cast<int>(ctrl::bold_bright), static_cast<int>(color));
 }
 
-#endif
+inline auto ansi_reset() -> std::string
+{
+    return std::format("\033[{}m", static_cast<int>(ctrl::reset));
+}
+
+// Output
+
+inline auto operator<<(std::ostream& os, const Vector2& v) -> std::ostream&
+{
+    os << "(" << v.x << ", " << v.y << ")";
+    return os;
+}
+
+inline auto operator<<(std::ostream& os, const Vector3& v) -> std::ostream&
+{
+    os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
+    return os;
+}
+
+// std::println doesn't work with mingw
+template <typename... Args>
+auto traceln(std::format_string<Args...> fmt, Args&&... args) -> void
+{
+    std::cout << std::format("[{}TRACE{}]: ", ansi_bold_fg(fg::cyan), ansi_reset()) << std::format(
+        fmt,
+        std::forward<Args>(args)...) << std::endl;
+}
+
+template <typename... Args>
+auto infoln(std::format_string<Args...> fmt, Args&&... args) -> void
+{
+    std::cout << std::format("[{}INFO{}]: ", ansi_bold_fg(fg::blue), ansi_reset()) << std::format(
+        fmt,
+        std::forward<Args>(args)...) << std::endl;
+}
+
+template <typename... Args>
+auto warnln(std::format_string<Args...> fmt, Args&&... args) -> void
+{
+    std::cout << std::format("[{}WARNING{}]: ", ansi_bold_fg(fg::yellow), ansi_reset()) << std::format(
+        fmt,
+        std::forward<Args>(args)...) << std::endl;
+}
+
+template <typename... Args>
+auto errln(std::format_string<Args...> fmt, Args&&... args) -> void
+{
+    std::cout << std::format("[{}ERROR{}]: ", ansi_bold_fg(fg::red), ansi_reset()) << std::format(
+        fmt,
+        std::forward<Args>(args)...) << std::endl;
+}
+
+#endif

raygui.patch

@@ -0,0 +1,53 @@
+diff --git a/src/raygui.h b/src/raygui.h
+index 03e4879..6986498 100644
+--- a/src/raygui.h
++++ b/src/raygui.h
+@@ -787,8 +787,8 @@ RAYGUIAPI int GuiCheckBox(Rectangle bounds, const char *text, bool *checked);
+ RAYGUIAPI int GuiComboBox(Rectangle bounds, const char *text, int *active);                            // Combo Box control
+ 
+ RAYGUIAPI int GuiDropdownBox(Rectangle bounds, const char *text, int *active, bool editMode);          // Dropdown Box control
+-RAYGUIAPI int GuiSpinner(Rectangle bounds, const char *text, int *value, int minValue, int maxValue, bool editMode); // Spinner control
+-RAYGUIAPI int GuiValueBox(Rectangle bounds, const char *text, int *value, int minValue, int maxValue, bool editMode); // Value Box control, updates input text with numbers
++RAYGUIAPI int GuiSpinner(Rectangle bounds, const char *text, const char *prefix, int *value, int minValue, int maxValue, bool editMode); // Spinner control
++RAYGUIAPI int GuiValueBox(Rectangle bounds, const char *text, const char *prefix, int *value, int minValue, int maxValue, bool editMode); // Value Box control, updates input text with numbers
+ RAYGUIAPI int GuiValueBoxFloat(Rectangle bounds, const char *text, char *textValue, float *value, bool editMode); // Value box control for float values
+ RAYGUIAPI int GuiTextBox(Rectangle bounds, char *text, int textSize, bool editMode);                   // Text Box control, updates input text
+ 
+@@ -2989,7 +2989,7 @@ bool GuiTextBoxMulti(Rectangle bounds, char *text, int textSize, bool editMode)
+ */
+ 
+ // Spinner control, returns selected value
+-int GuiSpinner(Rectangle bounds, const char *text, int *value, int minValue, int maxValue, bool editMode)
++int GuiSpinner(Rectangle bounds, const char *text, const char *prefix, int *value, int minValue, int maxValue, bool editMode)
+ {
+     int result = 1;
+     GuiState state = guiState;
+@@ -3045,7 +3045,7 @@ int GuiSpinner(Rectangle bounds, const char *text, int *value, int minValue, int
+ 
+     // Draw control
+     //--------------------------------------------------------------------
+-    result = GuiValueBox(valueBoxBounds, NULL, &tempValue, minValue, maxValue, editMode);
++    result = GuiValueBox(valueBoxBounds, NULL, prefix, &tempValue, minValue, maxValue, editMode);
+ 
+     // Draw value selector custom buttons
+     // NOTE: BORDER_WIDTH and TEXT_ALIGNMENT forced values
+@@ -3067,17 +3067,17 @@ int GuiSpinner(Rectangle bounds, const char *text, int *value, int minValue, int
+ 
+ // Value Box control, updates input text with numbers
+ // NOTE: Requires static variables: frameCounter
+-int GuiValueBox(Rectangle bounds, const char *text, int *value, int minValue, int maxValue, bool editMode)
++int GuiValueBox(Rectangle bounds, const char *text, const char *prefix, int *value, int minValue, int maxValue, bool editMode)
+ {
+     #if !defined(RAYGUI_VALUEBOX_MAX_CHARS)
+         #define RAYGUI_VALUEBOX_MAX_CHARS  32
+     #endif
+ 
+     int result = 0;
+     GuiState state = guiState;
+ 
+     char textValue[RAYGUI_VALUEBOX_MAX_CHARS + 1] = { 0 };
+-    snprintf(textValue, RAYGUI_VALUEBOX_MAX_CHARS + 1, "%i", *value);
++    snprintf(textValue, RAYGUI_VALUEBOX_MAX_CHARS + 1, "%s%i", prefix, *value);
+ 
+     Rectangle textBounds = { 0 };
+     if (text != NULL)

shader/edge_fragment.glsl

@@ -0,0 +1,9 @@
+#version 330
+
+uniform vec4 colDiffuse;
+out vec4 finalColor;
+
+void main()
+{
+    finalColor = colDiffuse;
+}

shader/edge_vertex.glsl

@@ -0,0 +1,9 @@
+#version 330
+
+in vec3 vertexPosition;
+uniform mat4 mvp;
+
+void main()
+{
+    gl_Position = mvp * vec4(vertexPosition, 1.0);
+}

shader/instancing_fragment.glsl

@@ -1,8 +1,9 @@
 #version 330
 
-uniform vec4 colDiffuse;
+in vec4 fragColor;
 out vec4 finalColor;
 
 void main() {
-    finalColor = colDiffuse;
-}
+    // Advanced coloring. CG lecture really paying off now
+    finalColor = fragColor;
+}

shader/instancing_vertex.glsl

@@ -1,10 +1,14 @@
 #version 330
 
-in vec3 vertexPosition;
-in mat4 instanceTransform;
+layout(location=0) in vec3 vertexPosition;
+layout(location=1) in mat4 instanceTransform;
+layout(location=5) in vec4 instanceColor;
 
 uniform mat4 mvp;
 
+out vec4 fragColor;
+
 void main() {
+     fragColor = instanceColor;
     gl_Position = mvp * instanceTransform * vec4(vertexPosition, 1.0);
-}
+}

src/backward.cpp

@@ -2,14 +2,14 @@
 //
 // On GNU/Linux, you have few choices to get the most out of your stack trace.
 //
-// By default you get:
-//	- object filename
-//	- function name
+// By default, you get:
+//  - object filename
+//  - function name
 //
 // In order to add:
-//	- source filename
-//	- line and column numbers
-//	- source code snippet (assuming the file is accessible)
+//  - source filename
+//  - line and column numbers
+//  - source code snippet (assuming the file is accessible)
 
 // Install one of the following libraries then uncomment one of the macro (or
 // better, add the detection of the lib and the macro definition in your build
@@ -33,16 +33,13 @@
 // - g++/clang++ -lunwind
 // #define BACKWARD_HAS_LIBUNWIND 1
 
-#include "config.hpp"
-
 #ifdef BACKWARD
 
-#include "backward.hpp"
-
-namespace backward {
-
-backward::SignalHandling sh;
+    #include "backward.hpp"
 
+namespace backward
+{
+SignalHandling sh;
 } // namespace backward
 
 #endif

src/bits.cpp

@@ -0,0 +1,15 @@
+#include "bits.hpp"
+
+auto print_bitmap(const u64 bitmap, const u8 w, const u8 h, const std::string& title) -> void {
+    traceln("{}:", title);
+    traceln("{}", std::string(2 * w - 1, '='));
+    for (size_t y = 0; y < h; ++y) {
+        std::cout << "         ";
+        for (size_t x = 0; x < w; ++x) {
+            std::cout << static_cast<int>(get_bits(bitmap, y * w + x, y * w + x)) << " ";
+        }
+        std::cout << "\n";
+    }
+    std::cout << std::flush;
+    traceln("{}", std::string(2 * w - 1, '='));
+}

src/camera.cpp

@@ -1,99 +0,0 @@
-#include "camera.hpp"
-#include "config.hpp"
-
-#include <raylib.h>
-#include <raymath.h>
-
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-auto OrbitCamera3D::HandleCameraInput() -> Vector2 {
-  Vector2 mouse = GetMousePosition();
-  if (mouse.x >= GetScreenWidth() / 2.0 && mouse.y >= MENU_HEIGHT) {
-    if (IsMouseButtonPressed(MOUSE_RIGHT_BUTTON)) {
-      rotating = true;
-      last_mouse = mouse;
-    } else if (IsMouseButtonPressed(MOUSE_LEFT_BUTTON)) {
-      panning = true;
-      target_lock = false;
-      last_mouse = mouse;
-    }
-
-    // Zoom
-    float wheel = GetMouseWheelMove();
-    if (IsKeyDown(KEY_LEFT_SHIFT)) {
-      distance -= wheel * ZOOM_SPEED * ZOOM_MULTIPLIER;
-    } else {
-      distance -= wheel * ZOOM_SPEED;
-    }
-  }
-
-  if (IsMouseButtonReleased(MOUSE_RIGHT_BUTTON)) {
-    rotating = false;
-  }
-  if (IsMouseButtonReleased(MOUSE_LEFT_BUTTON)) {
-    panning = false;
-  }
-
-  if (IsKeyPressed(KEY_L)) {
-    target_lock = !target_lock;
-  }
-  return mouse;
-}
-
-auto OrbitCamera3D::Update(const Vector3 &current_target) -> void {
-  Vector2 mouse = HandleCameraInput();
-
-  if (rotating) {
-    Vector2 dx = Vector2Subtract(mouse, last_mouse);
-    last_mouse = mouse;
-
-    angle_x -= dx.x * ROT_SPEED / 200.0;
-    angle_y += dx.y * ROT_SPEED / 200.0;
-
-    angle_y = Clamp(angle_y, -1.5, 1.5); // Prevent flipping
-  }
-
-  if (panning) {
-    Vector2 dx = Vector2Subtract(mouse, last_mouse);
-    last_mouse = mouse;
-
-    float speed;
-    if (IsKeyDown(KEY_LEFT_SHIFT)) {
-      speed = distance * PAN_SPEED / 1000.0 * PAN_MULTIPLIER;
-    } else {
-      speed = distance * PAN_SPEED / 1000.0;
-    }
-
-    // The panning needs to happen in camera coordinates, otherwise rotating the
-    // camera breaks it
-    Vector3 forward =
-        Vector3Normalize(Vector3Subtract(camera.target, camera.position));
-    Vector3 right = Vector3Normalize(Vector3CrossProduct(forward, camera.up));
-    Vector3 up = Vector3Normalize(Vector3CrossProduct(right, forward));
-
-    Vector3 offset = Vector3Add(Vector3Scale(right, -dx.x * speed),
-                                Vector3Scale(up, dx.y * speed));
-
-    target = Vector3Add(target, offset);
-  }
-
-  if (target_lock) {
-    target = Vector3MoveTowards(
-        target, current_target,
-        CAMERA_SMOOTH_SPEED * GetFrameTime() *
-            Vector3Length(Vector3Subtract(target, current_target)));
-  }
-
-  distance = Clamp(distance, MIN_CAMERA_DISTANCE, MAX_CAMERA_DISTANCE);
-
-  // Spherical coordinates
-  float x = cos(angle_y) * sin(angle_x) * distance;
-  float y = sin(angle_y) * distance;
-  float z = cos(angle_y) * cos(angle_x) * distance;
-
-  camera.position = Vector3Add(target, Vector3(x, y, z));
-  camera.target = target;
-}

src/cpu_layout_engine.cpp

@@ -0,0 +1,236 @@
+#include "cpu_layout_engine.hpp"
+#include "config.hpp"
+#include "cpu_spring_system.hpp"
+#include "util.hpp"
+
+#include <chrono>
+#include <raylib.h>
+#include <raymath.h>
+#include <utility>
+#include <vector>
+
+#ifdef ASYNC_OCTREE
+auto cpu_layout_engine::set_octree_pool_thread_name(size_t idx) -> void
+{
+    BS::this_thread::set_os_thread_name(std::format("octree-{}", idx));
+    // traceln("Using thread \"{}\"", BS::this_thread::get_os_thread_name().value_or("INVALID NAME"));
+}
+#endif
+
+auto cpu_layout_engine::physics_thread(physics_state& state, const threadpool thread_pool) -> void
+{
+    cpu_spring_system mass_springs;
+
+    #ifdef ASYNC_OCTREE
+    BS::this_thread::set_os_thread_name("physics");
+
+    BS::thread_pool<> octree_thread(1, set_octree_pool_thread_name);
+    std::future<void> octree_future;
+    octree tree_buffer;
+    size_t last_mass_count = 0;
+    #endif
+
+    const auto visitor = overloads{
+        [&](const struct add_mass&)
+        {
+            mass_springs.add_mass();
+        },
+        [&](const struct add_spring& as)
+        {
+            mass_springs.add_spring(as.a, as.b);
+        },
+        [&](const struct clear_graph&)
+        {
+            mass_springs.clear();
+        },
+    };
+
+    std::chrono::time_point last = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> physics_accumulator(0);
+    std::chrono::duration<double> ups_accumulator(0);
+    int loop_iterations = 0;
+
+    while (state.running.load()) {
+        #ifdef TRACY
+        FrameMarkStart("PhysicsThread");
+        #endif
+
+        // Time tracking
+        std::chrono::time_point now = std::chrono::high_resolution_clock::now();
+        const std::chrono::duration<double> deltatime = now - last;
+        physics_accumulator += deltatime;
+        ups_accumulator += deltatime;
+        last = now;
+
+        // Handle queued commands
+        {
+            #ifdef TRACY
+            std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
+            #else
+            std::lock_guard<std::mutex> lock(state.command_mtx);
+            #endif
+            while (!state.pending_commands.empty()) {
+                command& cmd = state.pending_commands.front();
+                cmd.visit(visitor);
+                state.pending_commands.pop();
+            }
+        }
+
+        if (mass_springs.positions.empty()) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+
+        // Physics update
+        if (physics_accumulator.count() > TIMESTEP) {
+            #ifdef ASYNC_OCTREE
+            // Snapshot the positions so mass_springs is not mutating the vector while the octree is building
+            std::vector<Vector3> positions = mass_springs.positions;
+
+            // Start building the octree for the next physics update.
+            // Move the snapshot into the closure so it doesn't get captured by reference (don't use [&])
+            octree_future = octree_thread.submit_task([&tree_buffer, &thread_pool, positions = std::move(positions)]()
+            {
+                octree::build_octree_morton(tree_buffer, positions, thread_pool);
+            });
+
+            // Rebuild the tree synchronously if we changed the number of masses to not use
+            // an empty tree from the last frame in the frame where the graph was generated
+            if (last_mass_count != mass_springs.positions.size()) {
+                traceln("Rebuilding octree synchronously because graph size changed");
+                octree::build_octree_morton(mass_springs.tree, mass_springs.positions, thread_pool);
+                last_mass_count = mass_springs.positions.size();
+            }
+            #else
+            octree::build_octree_morton(mass_springs.tree, mass_springs.positions, thread_pool);
+            #endif
+
+            mass_springs.clear_forces();
+            mass_springs.calculate_spring_forces(thread_pool);
+            mass_springs.calculate_repulsion_forces(thread_pool);
+            mass_springs.update(TIMESTEP * SIM_SPEED, thread_pool);
+
+            // This is only helpful if we're drawing a grid at (0, 0, 0). Otherwise, it's just
+            // expensive and yields no benefit since we can lock the camera to the center of mass
+            // cheaply.
+            // mass_springs.center_masses(thread_pool);
+
+            ++loop_iterations;
+            physics_accumulator -= std::chrono::duration<double>(TIMESTEP);
+        }
+
+        #ifdef ASYNC_OCTREE
+        // Wait for the octree to be built
+        if (octree_future.valid()) {
+            octree_future.wait();
+            octree_future = std::future<void>{};
+            std::swap(mass_springs.tree, tree_buffer);
+        }
+        #endif
+
+        // Publish the positions for the renderer (copy)
+        #ifdef TRACY
+        FrameMarkStart("PhysicsThreadProduceLock");
+        #endif
+        {
+            #ifdef TRACY
+            std::unique_lock<LockableBase(std::mutex)> lock(state.data_mtx);
+            #else
+            std::unique_lock<std::mutex> lock(state.data_mtx);
+            #endif
+            state.data_consumed_cnd.wait(lock,
+                                         [&]
+                                         {
+                                             return state.data_consumed || !state.running.load();
+                                         });
+            if (!state.running.load()) {
+                // Running turned false while we were waiting for the condition
+                break;
+            }
+
+            if (ups_accumulator.count() > 1.0) {
+                // Update each second
+                state.ups = loop_iterations;
+                loop_iterations = 0;
+                ups_accumulator = std::chrono::duration<double>(0);
+            }
+            if (mass_springs.tree.empty()) {
+                state.mass_center = Vector3Zero();
+            } else {
+                state.mass_center = mass_springs.tree.root().mass_center;
+            }
+
+            state.masses.clear();
+            state.masses.reserve(mass_springs.positions.size());
+            for (const Vector3& pos : mass_springs.positions) {
+                state.masses.emplace_back(pos);
+            }
+
+            state.mass_count = mass_springs.positions.size();
+            state.spring_count = mass_springs.springs.size();
+
+            state.data_ready = true;
+            state.data_consumed = false;
+        }
+        // Notify the rendering thread that new data is available
+        state.data_ready_cnd.notify_all();
+
+        #ifdef TRACY
+        FrameMarkEnd("PhysicsThreadProduceLock");
+        FrameMarkEnd("PhysicsThread");
+        #endif
+    }
+}
+
+auto cpu_layout_engine::clear_cmd() -> void
+{
+    {
+        #ifdef TRACY
+        std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
+        #else
+        std::lock_guard<std::mutex> lock(state.command_mtx);
+        #endif
+        state.pending_commands.emplace(clear_graph{});
+    }
+}
+
+auto cpu_layout_engine::add_mass_cmd() -> void
+{
+    {
+        #ifdef TRACY
+        std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
+        #else
+        std::lock_guard<std::mutex> lock(state.command_mtx);
+        #endif
+        state.pending_commands.emplace(add_mass{});
+    }
+}
+
+auto cpu_layout_engine::add_spring_cmd(const size_t a, const size_t b) -> void
+{
+    {
+        #ifdef TRACY
+        std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
+        #else
+        std::lock_guard<std::mutex> lock(state.command_mtx);
+        #endif
+        state.pending_commands.emplace(add_spring{a, b});
+    }
+}
+
+auto cpu_layout_engine::add_mass_springs_cmd(const size_t num_masses, const std::vector<spring>& springs) -> void
+{
+    {
+        #ifdef TRACY
+        std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
+        #else
+        std::lock_guard<std::mutex> lock(state.command_mtx);
+        #endif
+        for (size_t i = 0; i < num_masses; ++i) {
+            state.pending_commands.emplace(add_mass{});
+        }
+        for (const auto& [from, to] : springs) {
+            state.pending_commands.emplace(add_spring{from, to});
+        }
+    }
+}

src/cpu_spring_system.cpp

@@ -0,0 +1,209 @@
+#include "cpu_spring_system.hpp"
+#include "config.hpp"
+
+#include <cfloat>
+#include <cstring>
+
+auto cpu_spring_system::clear() -> void
+{
+    positions.clear();
+    previous_positions.clear();
+    velocities.clear();
+    forces.clear();
+    springs.clear();
+    tree.clear();
+}
+
+auto cpu_spring_system::add_mass() -> void
+{
+    // Adding all positions to (0, 0, 0) breaks the octree
+
+    // Done when adding springs
+    // Vector3 position{
+    //   static_cast<float>(GetRandomValue(-100, 100)), static_cast<float>(GetRandomValue(-100,
+    //   100)), static_cast<float>(GetRandomValue(-100, 100))
+    // };
+    // position = Vector3Scale(Vector3Normalize(position), REST_LENGTH * 2.0);
+
+    positions.emplace_back(Vector3Zero());
+    previous_positions.emplace_back(Vector3Zero());
+    velocities.emplace_back(Vector3Zero());
+    forces.emplace_back(Vector3Zero());
+}
+
+auto cpu_spring_system::add_spring(size_t a, size_t b) -> void
+{
+    // Update masses to be located along a random walk when adding the springs
+    const Vector3& mass_a = positions[a];
+    const Vector3& mass_b = positions[b];
+
+    Vector3 offset{
+        static_cast<float>(GetRandomValue(-100, 100)),
+        static_cast<float>(GetRandomValue(-100, 100)),
+        static_cast<float>(GetRandomValue(-100, 100))
+    };
+
+    // By spawning the masses close together, we "explode" them naturally, so they cluster faster (also looks cool)
+    offset = Vector3Normalize(offset) * REST_LENGTH * 0.1;
+
+    // If the offset moves the mass closer to the current center of mass, flip it
+    if (!tree.empty()) {
+        const Vector3 mass_center_direction = Vector3Subtract(positions[a], tree.root().mass_center);
+        const float mass_center_distance = Vector3Length(mass_center_direction);
+
+        if (mass_center_distance > 0 && Vector3DotProduct(offset, mass_center_direction) < 0.0f) {
+            offset = Vector3Negate(offset);
+        }
+    }
+
+    positions[b] = mass_a + offset;
+    previous_positions[b] = mass_b;
+
+    // infoln("Adding spring: ({}, {}, {})->({}, {}, {})", mass_a.position.x, mass_a.position.y,
+    // mass_a.position.z,
+    //        mass_b.position.x, mass_b.position.y, mass_b.position.z);
+
+    springs.emplace_back(a, b);
+}
+
+auto cpu_spring_system::clear_forces() -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    memset(forces.data(), 0, forces.size() * sizeof(Vector3));
+}
+
+auto cpu_spring_system::calculate_spring_force(const size_t s) -> void
+{
+    const spring _s = springs[s];
+    const Vector3 a_pos = positions[_s.first];
+    const Vector3 b_pos = positions[_s.second];
+    const Vector3 a_vel = velocities[_s.first];
+    const Vector3 b_vel = velocities[_s.second];
+
+    const Vector3 delta_pos = a_pos - b_pos;
+    const Vector3 delta_vel = a_vel - b_vel;
+
+    const float sq_len = Vector3DotProduct(delta_pos, delta_pos);
+    const float inv_len = 1.0f / sqrt(sq_len);
+    const float len = sq_len * inv_len;
+
+    const float hooke = SPRING_K * (len - REST_LENGTH);
+    const float dampening = DAMPENING_K * Vector3DotProduct(delta_vel, delta_pos) * inv_len;
+
+    const Vector3 a_force = Vector3Scale(delta_pos, -(hooke + dampening) * inv_len);
+    const Vector3 b_force = a_force * -1.0f;
+
+    forces[_s.first] += a_force;
+    forces[_s.second] += b_force;
+}
+
+auto cpu_spring_system::calculate_spring_forces(const threadpool thread_pool) -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    const auto solve_spring_force = [&](const int i)
+    {
+        calculate_spring_force(i);
+    };
+
+    if (thread_pool) {
+        (*thread_pool)->submit_loop(0, springs.size(), solve_spring_force, SMALL_TASK_BLOCK_SIZE).wait();
+    } else {
+        for (size_t i = 0; i < springs.size(); ++i) {
+            solve_spring_force(i);
+        }
+    }
+}
+
+auto cpu_spring_system::calculate_repulsion_forces(const threadpool thread_pool) -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    const auto solve_octree = [&](const int i)
+    {
+        const Vector3 force = tree.calculate_force_morton(0, positions[i], i);
+        forces[i] += force;
+    };
+
+    // Calculate forces using Barnes-Hut
+    if (thread_pool) {
+        (*thread_pool)->submit_loop(0, positions.size(), solve_octree, LARGE_TASK_BLOCK_SIZE).wait();
+    } else {
+        for (size_t i = 0; i < positions.size(); ++i) {
+            solve_octree(i);
+        }
+    }
+}
+
+auto cpu_spring_system::integrate_velocity(const size_t m, const float dt) -> void
+{
+    const Vector3 acc = forces[m] / MASS;
+    velocities[m] += acc * dt;
+}
+
+auto cpu_spring_system::integrate_position(const size_t m, const float dt) -> void
+{
+    previous_positions[m] = positions[m];
+    positions[m] += velocities[m] * dt;
+}
+
+auto cpu_spring_system::verlet_update(const size_t m, const float dt) -> void
+{
+    const Vector3 acc = (forces[m] / MASS) * dt * dt;
+    const Vector3 pos = positions[m];
+
+    Vector3 delta_pos = pos - previous_positions[m];
+    delta_pos *= 1.0 - VERLET_DAMPENING; // Minimal dampening
+
+    positions[m] += delta_pos + acc;
+    previous_positions[m] = pos;
+}
+
+auto cpu_spring_system::update(const float dt, const threadpool thread_pool) -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    const auto update = [&](const int i)
+    {
+        verlet_update(i, dt);
+    };
+
+    if (thread_pool) {
+        (*thread_pool)->submit_loop(0, positions.size(), update, SMALL_TASK_BLOCK_SIZE).wait();
+    } else {
+        for (size_t i = 0; i < positions.size(); ++i) {
+            update(i);
+        }
+    }
+}
+
+auto cpu_spring_system::center_masses(const threadpool thread_pool) -> void
+{
+    Vector3 mean = Vector3Zero();
+    for (const Vector3& pos : positions) {
+        mean += pos;
+    }
+    mean /= static_cast<float>(positions.size());
+
+    const auto center_mass = [&](const int i)
+    {
+        positions[i] -= mean;
+    };
+
+    if (thread_pool) {
+        (*thread_pool)->submit_loop(0, positions.size(), center_mass, SMALL_TASK_BLOCK_SIZE).wait();
+    } else {
+        for (size_t i = 0; i < positions.size(); ++i) {
+            center_mass(i);
+        }
+    }
+}

src/distance.cpp

@@ -1,86 +0,0 @@
-#include "distance.hpp"
-#include "config.hpp"
-
-#include <cstddef>
-#include <queue>
-
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-auto DistanceResult::Clear() -> void {
-  distances.clear();
-  parents.clear();
-  nearest_targets.clear();
-}
-
-auto DistanceResult::Empty() -> bool {
-  return distances.empty() || parents.empty() || nearest_targets.empty();
-}
-
-auto CalculateDistances(
-    std::size_t node_count,
-    const std::vector<std::pair<std::size_t, std::size_t>> &edges,
-    const std::vector<std::size_t> &targets) -> DistanceResult {
-
-  // Build a list of adjacent nodes to speed up BFS
-  std::vector<std::vector<std::size_t>> adjacency(node_count);
-  for (const auto &[from, to] : edges) {
-    adjacency[from].push_back(to);
-    adjacency[to].push_back(from);
-  }
-  // for (size_t i = 0; i < adjacency.size(); ++i) {
-  //   std::cout << "Node " << i << "'s neighbors: ";
-  //   for (const auto &neighbor : adjacency[i]) {
-  //     std::cout << neighbor;
-  //   }
-  //   std::cout << "\n";
-  // }
-  // std::cout << std::endl;
-
-  std::vector<int> distances(node_count, -1);
-  std::vector<std::size_t> parents(node_count, -1);
-  std::vector<std::size_t> nearest_targets(node_count, -1);
-
-  std::queue<std::size_t> queue;
-  for (std::size_t target : targets) {
-    distances[target] = 0;
-    nearest_targets[target] = target;
-    queue.push(target);
-  }
-
-  while (!queue.empty()) {
-    std::size_t current = queue.front();
-    queue.pop();
-
-    for (std::size_t neighbor : adjacency[current]) {
-      if (distances[neighbor] == -1) {
-        // If distance is -1 we haven't visited the node yet
-        distances[neighbor] = distances[current] + 1;
-        parents[neighbor] = current;
-        nearest_targets[neighbor] = nearest_targets[current];
-
-        queue.push(neighbor);
-      }
-    }
-  }
-
-  return {distances, parents, nearest_targets};
-}
-
-auto GetPath(const DistanceResult &result, std::size_t source)
-    -> std::vector<std::size_t> {
-  if (result.distances[source] == -1) {
-    // Unreachable
-    return {};
-  }
-
-  std::vector<std::size_t> path;
-  for (std::size_t n = source; n != static_cast<std::size_t>(-1);
-       n = result.parents[n]) {
-    path.push_back(n);
-  }
-
-  return path;
-}

src/graph_distances.cpp

@@ -0,0 +1,69 @@
+#include "graph_distances.hpp"
+
+#include <queue>
+
+auto graph_distances::clear() -> void
+{
+    distances.clear();
+    parents.clear();
+    nearest_targets.clear();
+}
+
+auto graph_distances::empty() const -> bool
+{
+    return distances.empty() || parents.empty() || nearest_targets.empty();
+}
+
+auto graph_distances::calculate_distances(const size_t node_count,
+                                          const std::vector<spring>& edges,
+                                          const std::vector<size_t>& targets) -> void
+{
+    // Build a list of adjacent nodes to speed up BFS
+    std::vector<std::vector<size_t>> adjacency(node_count);
+    for (const auto& [from, to] : edges) {
+        adjacency[from].push_back(to);
+        adjacency[to].push_back(from);
+    }
+
+    distances = std::vector<int>(node_count, -1);
+    parents = std::vector<size_t>(node_count, -1);
+    nearest_targets = std::vector<size_t>(node_count, -1);
+
+    std::queue<size_t> queue;
+    for (size_t target : targets) {
+        distances[target] = 0;
+        nearest_targets[target] = target;
+        queue.push(target);
+    }
+
+    while (!queue.empty()) {
+        const size_t current = queue.front();
+        queue.pop();
+
+        for (size_t neighbor : adjacency[current]) {
+            if (distances[neighbor] == -1) {
+                // If distance is -1 we haven't visited the node yet
+                distances[neighbor] = distances[current] + 1;
+                parents[neighbor] = current;
+                nearest_targets[neighbor] = nearest_targets[current];
+
+                queue.push(neighbor);
+            }
+        }
+    }
+}
+
+auto graph_distances::get_shortest_path(const size_t source) const -> std::vector<size_t>
+{
+    if (empty() || distances[source] == -1) {
+        // Unreachable
+        return {};
+    }
+
+    std::vector<size_t> path;
+    for (size_t n = source; n != static_cast<size_t>(-1); n = parents[n]) {
+        path.push_back(n);
+    }
+
+    return path;
+}

src/input.cpp

@@ -1,197 +0,0 @@
-#include "input.hpp"
-#include "config.hpp"
-
-#include <algorithm>
-#include <raylib.h>
-
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-auto InputHandler::HandleMouseHover() -> void {
-  const int board_width = GetScreenWidth() / 2.0 - 2 * BOARD_PADDING;
-  const int board_height = GetScreenHeight() - MENU_HEIGHT - 2 * BOARD_PADDING;
-  int block_size = std::min(board_width / state.current_state.width,
-                            board_height / state.current_state.height) -
-                   2 * BLOCK_PADDING;
-  int x_offset = (board_width - (block_size + 2 * BLOCK_PADDING) *
-                                    state.current_state.width) /
-                 2.0;
-  int y_offset = (board_height - (block_size + 2 * BLOCK_PADDING) *
-                                     state.current_state.height) /
-                 2.0;
-
-  Vector2 m = GetMousePosition();
-  if (m.x < x_offset) {
-    hov_x = 100;
-  } else {
-    hov_x = (m.x - x_offset) / (block_size + 2 * BLOCK_PADDING);
-  }
-  if (m.y - MENU_HEIGHT < y_offset) {
-    hov_y = 100;
-  } else {
-    hov_y = (m.y - MENU_HEIGHT - y_offset) / (block_size + 2 * BLOCK_PADDING);
-  }
-}
-
-auto InputHandler::HandleMouse() -> void {
-  if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) {
-    // If we clicked a block...
-    if (state.current_state.GetBlock(hov_x, hov_y).IsValid()) {
-      sel_x = hov_x;
-      sel_y = hov_y;
-    }
-    // If we clicked empty space...
-    else {
-      // Select a position
-      if (!has_block_add_xy) {
-        if (hov_x >= 0 && hov_x < state.current_state.width && hov_y >= 0 &&
-            hov_y < state.current_state.height) {
-          block_add_x = hov_x;
-          block_add_y = hov_y;
-          has_block_add_xy = true;
-        }
-      }
-      // If we have already selected a position
-      else {
-        int block_add_width = hov_x - block_add_x + 1;
-        int block_add_height = hov_y - block_add_y + 1;
-        if (block_add_width <= 0 || block_add_height <= 0) {
-          block_add_x = -1;
-          block_add_y = -1;
-          has_block_add_xy = false;
-        } else if (block_add_x >= 0 &&
-                   block_add_x + block_add_width <= state.current_state.width &&
-                   block_add_y >= 0 &&
-                   block_add_y + block_add_height <=
-                       state.current_state.height) {
-          bool success = state.current_state.AddBlock(
-              Block(block_add_x, block_add_y, block_add_width, block_add_height,
-                    false));
-
-          if (success) {
-            block_add_x = -1;
-            block_add_y = -1;
-            has_block_add_xy = false;
-            state.ClearGraph();
-            state.edited = true;
-          }
-        }
-      }
-    }
-  } else if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) {
-    if (state.current_state.RemoveBlock(hov_x, hov_y)) {
-      state.ClearGraph();
-      state.edited = true;
-    } else if (has_block_add_xy) {
-      block_add_x = -1;
-      block_add_y = -1;
-      has_block_add_xy = false;
-    }
-  } else if (IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) {
-    if (hov_x >= 0 && hov_x < state.current_state.width && hov_y >= 0 &&
-        hov_y < state.current_state.height) {
-      if (state.current_state.SetGoal(hov_x, hov_y)) {
-        // We can't just call state.FindWinningStates() because the
-        // state is entirely different if it has a different win condition.
-        state.ClearGraph();
-      }
-    }
-  }
-}
-
-auto InputHandler::HandleKeys() -> void {
-  if (IsKeyPressed(KEY_W)) {
-    if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::NOR)) {
-      sel_y--;
-    }
-  } else if (IsKeyPressed(KEY_A)) {
-    if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::WES)) {
-      sel_x--;
-    }
-  } else if (IsKeyPressed(KEY_S)) {
-    if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::SOU)) {
-      sel_y++;
-    }
-  } else if (IsKeyPressed(KEY_D)) {
-    if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::EAS)) {
-      sel_x++;
-    }
-  } else if (IsKeyPressed(KEY_P)) {
-    std::cout << "State: " << state.current_state.state << std::endl;
-    Block sel = state.current_state.GetBlock(sel_x, sel_y);
-    int idx = state.current_state.GetIndex(sel.x, sel.y) - 5;
-    if (sel.IsValid()) {
-      std::cout << "Sel:   " << state.current_state.state.substr(0, 5)
-                << std::string(idx, '.') << sel.ToString()
-                << std::string(state.current_state.state.length() - idx - 7,
-                               '.')
-                << std::endl;
-    }
-  } else if (IsKeyPressed(KEY_N)) {
-    block_add_x = -1;
-    block_add_y = -1;
-    has_block_add_xy = false;
-    state.PreviousPreset();
-  } else if (IsKeyPressed(KEY_M)) {
-    block_add_x = -1;
-    block_add_y = -1;
-    has_block_add_xy = false;
-    state.NextPreset();
-  } else if (IsKeyPressed(KEY_R)) {
-    state.ResetState();
-  } else if (IsKeyPressed(KEY_G)) {
-    state.FillGraph();
-  } else if (IsKeyPressed(KEY_C)) {
-    state.ClearGraph();
-  } else if (IsKeyPressed(KEY_I)) {
-    mark_solutions = !mark_solutions;
-  } else if (IsKeyPressed(KEY_O)) {
-    connect_solutions = !connect_solutions;
-  } else if (IsKeyPressed(KEY_U)) {
-    mark_path = !mark_path;
-  } else if (IsKeyPressed(KEY_SPACE)) {
-    state.NextPath();
-  } else if (IsKeyPressed(KEY_V)) {
-    state.GoToWorst();
-  } else if (IsKeyPressed(KEY_B)) {
-    state.GoToNearestTarget();
-  } else if (IsKeyPressed(KEY_BACKSPACE)) {
-    state.PopHistory();
-  } else if (IsKeyPressed(KEY_F)) {
-    state.current_state.ToggleRestricted();
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_T)) {
-    state.current_state.ToggleTarget(sel_x, sel_y);
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_Y)) {
-    state.current_state.ToggleWall(sel_x, sel_y);
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_LEFT) && state.current_state.width > 1) {
-    state.current_state = state.current_state.RemoveColumn();
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_RIGHT) && state.current_state.width < 9) {
-    state.current_state = state.current_state.AddColumn();
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_UP) && state.current_state.height > 1) {
-    state.current_state = state.current_state.RemoveRow();
-    state.ClearGraph();
-    state.edited = true;
-  } else if (IsKeyPressed(KEY_DOWN) && state.current_state.height < 9) {
-    state.current_state = state.current_state.AddRow();
-    state.ClearGraph();
-    state.edited = true;
-  }
-}
-
-auto InputHandler::HandleInput() -> void {
-  HandleMouseHover();
-  HandleMouse();
-  HandleKeys();
-}

src/input_handler.cpp

@@ -0,0 +1,639 @@
+#include "input_handler.hpp"
+#include "config.hpp"
+
+#include <raylib.h>
+
+auto input_handler::init_handlers() -> void
+{
+    // The order matters if multiple handlers are registered to the same key
+
+    register_generic_handler(&input_handler::camera_pan);
+    register_generic_handler(&input_handler::camera_rotate);
+    register_generic_handler(&input_handler::camera_zoom);
+    register_generic_handler(&input_handler::camera_fov);
+    register_generic_handler(&input_handler::mouse_hover);
+
+    register_mouse_pressed_handler(MOUSE_BUTTON_LEFT, &input_handler::camera_start_pan);
+    register_mouse_pressed_handler(MOUSE_BUTTON_LEFT, &input_handler::select_block);
+    register_mouse_pressed_handler(MOUSE_BUTTON_LEFT, &input_handler::add_block);
+    register_mouse_pressed_handler(MOUSE_BUTTON_LEFT, &input_handler::start_add_block);
+    register_mouse_pressed_handler(MOUSE_BUTTON_MIDDLE, &input_handler::place_goal);
+    register_mouse_pressed_handler(MOUSE_BUTTON_MIDDLE, &input_handler::select_state);
+    register_mouse_pressed_handler(MOUSE_BUTTON_RIGHT, &input_handler::camera_start_rotate);
+    register_mouse_pressed_handler(MOUSE_BUTTON_RIGHT, &input_handler::remove_block);
+    register_mouse_pressed_handler(MOUSE_BUTTON_RIGHT, &input_handler::clear_add_block);
+
+    register_mouse_released_handler(MOUSE_BUTTON_LEFT, &input_handler::camera_stop_pan);
+    register_mouse_released_handler(MOUSE_BUTTON_RIGHT, &input_handler::camera_stop_rotate);
+
+    register_key_pressed_handler(KEY_W, &input_handler::move_block_nor);
+    register_key_pressed_handler(KEY_D, &input_handler::move_block_eas);
+    register_key_pressed_handler(KEY_S, &input_handler::move_block_sou);
+    register_key_pressed_handler(KEY_A, &input_handler::move_block_wes);
+
+    register_key_pressed_handler(KEY_N, &input_handler::load_previous_preset);
+    register_key_pressed_handler(KEY_M, &input_handler::load_next_preset);
+    register_key_pressed_handler(KEY_R, &input_handler::goto_starting_state);
+    register_key_pressed_handler(KEY_V, &input_handler::goto_most_distant_state);
+    register_key_pressed_handler(KEY_B, &input_handler::goto_closest_target_state);
+    register_key_pressed_handler(KEY_SPACE, &input_handler::goto_optimal_next_state);
+    register_key_pressed_handler(KEY_BACKSPACE, &input_handler::goto_previous_state);
+
+    register_key_pressed_handler(KEY_G, &input_handler::populate_graph);
+    register_key_pressed_handler(KEY_C, &input_handler::clear_graph);
+    register_key_pressed_handler(KEY_I, &input_handler::toggle_mark_solutions);
+    register_key_pressed_handler(KEY_O, &input_handler::toggle_connect_solutions);
+    register_key_pressed_handler(KEY_Z, &input_handler::toggle_color_by_distance);
+
+    register_key_pressed_handler(KEY_TAB, &input_handler::toggle_editing);
+    register_key_pressed_handler(KEY_F, &input_handler::toggle_restricted_movement);
+    register_key_pressed_handler(KEY_T, &input_handler::toggle_target_block);
+    register_key_pressed_handler(KEY_Y, &input_handler::toggle_wall_block);
+    register_key_pressed_handler(KEY_UP, &input_handler::add_board_row);
+    register_key_pressed_handler(KEY_RIGHT, &input_handler::add_board_column);
+    register_key_pressed_handler(KEY_DOWN, &input_handler::remove_board_row);
+    register_key_pressed_handler(KEY_LEFT, &input_handler::remove_board_column);
+    register_key_pressed_handler(KEY_X, &input_handler::clear_goal);
+    register_key_pressed_handler(KEY_P, &input_handler::print_state);
+    register_key_pressed_handler(KEY_S, &input_handler::save_preset); // + CTRL
+
+    register_key_pressed_handler(KEY_L, &input_handler::toggle_camera_lock);
+    register_key_pressed_handler(KEY_LEFT_ALT, &input_handler::toggle_camera_projection);
+    register_key_pressed_handler(KEY_U, &input_handler::toggle_camera_mass_center_lock);
+}
+
+auto input_handler::mouse_in_menu_pane() const -> bool
+{
+    return mouse.y < MENU_HEIGHT;
+}
+
+auto input_handler::mouse_in_board_pane() const -> bool
+{
+    return mouse.x < GetScreenWidth() / 2.0 && mouse.y >= MENU_HEIGHT;
+}
+
+auto input_handler::mouse_in_graph_pane() const -> bool
+{
+    return mouse.x >= GetScreenWidth() / 2.0 && mouse.y >= MENU_HEIGHT;
+}
+
+auto input_handler::mouse_hover() -> void
+{
+    last_mouse = mouse;
+    mouse = GetMousePosition();
+}
+
+auto input_handler::camera_start_pan() -> void
+{
+    if (!mouse_in_graph_pane()) {
+        return;
+    }
+
+    camera_panning = true;
+    // Enable this if the camera should be pannable even when locked (releasing the lock in the
+    // process): camera_lock = false;
+}
+
+auto input_handler::camera_pan() const -> void
+{
+    if (camera_panning) {
+        camera.pan(last_mouse, mouse);
+    }
+}
+
+auto input_handler::camera_stop_pan() -> void
+{
+    camera_panning = false;
+}
+
+auto input_handler::camera_start_rotate() -> void
+{
+    if (!mouse_in_graph_pane()) {
+        return;
+    }
+
+    camera_rotating = true;
+}
+
+auto input_handler::camera_rotate() const -> void
+{
+    if (camera_rotating) {
+        camera.rotate(last_mouse, mouse);
+    }
+}
+
+auto input_handler::camera_stop_rotate() -> void
+{
+    camera_rotating = false;
+}
+
+auto input_handler::camera_zoom() const -> void
+{
+    if (!mouse_in_graph_pane() || IsKeyDown(KEY_LEFT_CONTROL) || camera.projection == CAMERA_ORTHOGRAPHIC) {
+        return;
+    }
+
+    const float wheel = GetMouseWheelMove();
+
+    if (IsKeyDown(KEY_LEFT_SHIFT)) {
+        camera.distance -= wheel * ZOOM_SPEED * ZOOM_MULTIPLIER;
+    } else {
+        camera.distance -= wheel * ZOOM_SPEED;
+    }
+}
+
+auto input_handler::camera_fov() const -> void
+{
+    if (!mouse_in_graph_pane() || !IsKeyDown(KEY_LEFT_CONTROL)) {
+        return;
+    }
+
+    const float wheel = GetMouseWheelMove();
+
+    if (IsKeyDown(KEY_LEFT_SHIFT)) {
+        camera.fov -= wheel * FOV_SPEED * FOV_MULTIPLIER;
+    } else {
+        camera.fov -= wheel * FOV_SPEED;
+    }
+}
+
+auto input_handler::select_block() -> void
+{
+    const puzzle& current = state.get_current_state();
+    if (current.try_get_block(hov_x, hov_y)) {
+        sel_x = hov_x;
+        sel_y = hov_y;
+    }
+}
+
+auto input_handler::start_add_block() -> void
+{
+    const puzzle& current = state.get_current_state();
+    if (!editing || current.try_get_block(hov_x, hov_y) || has_block_add_xy || current.block_count() >=
+        puzzle::MAX_BLOCKS) {
+        return;
+    }
+
+    if (hov_x >= 0 && hov_x < current.get_width() && hov_y >= 0 && hov_y < current.get_height()) {
+        block_add_x = hov_x;
+        block_add_y = hov_y;
+        has_block_add_xy = true;
+    }
+}
+
+auto input_handler::clear_add_block() -> void
+{
+    if (!editing || !has_block_add_xy) {
+        return;
+    }
+
+    block_add_x = -1;
+    block_add_y = -1;
+    has_block_add_xy = false;
+}
+
+auto input_handler::add_block() -> void
+{
+    const puzzle& current = state.get_current_state();
+    if (!editing || current.try_get_block(hov_x, hov_y) || !has_block_add_xy || current.block_count() >=
+        puzzle::MAX_BLOCKS) {
+        return;
+    }
+
+    const int block_add_width = hov_x - block_add_x + 1;
+    const int block_add_height = hov_y - block_add_y + 1;
+    if (block_add_width <= 0 || block_add_height <= 0) {
+        block_add_x = -1;
+        block_add_y = -1;
+        has_block_add_xy = false;
+    } else if (current.covers(block_add_x, block_add_y, block_add_width, block_add_height)) {
+        const std::optional<puzzle>& next = current.try_add_block(
+            block(block_add_x, block_add_y, block_add_width, block_add_height, false));
+
+        if (next) {
+            sel_x = block_add_x;
+            sel_y = block_add_y;
+            block_add_x = -1;
+            block_add_y = -1;
+            has_block_add_xy = false;
+            state.edit_starting_state(*next);
+        }
+    }
+}
+
+auto input_handler::remove_block() -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<block>& b = current.try_get_block(hov_x, hov_y);
+    if (!editing || has_block_add_xy || !b) {
+        return;
+    }
+
+    const std::optional<puzzle>& next = current.try_remove_block(hov_x, hov_y);
+    if (!next) {
+        return;
+    }
+
+    // Reset selection if we removed the selected block
+    if (b->covers(sel_x, sel_y)) {
+        sel_x = 0;
+        sel_y = 0;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::place_goal() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    if (!editing || !mouse_in_board_pane() || !current.covers(hov_x, hov_y)) {
+        return;
+    }
+
+    const std::optional<puzzle>& next = current.try_set_goal(hov_x, hov_y);
+    if (!next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::select_state() const -> void
+{
+    if (!mouse_in_graph_pane() || collision_mass == static_cast<size_t>(-1)) {
+        return;
+    }
+
+    const puzzle& selected = state.get_state(collision_mass);
+    state.update_current_state(selected);
+}
+
+auto input_handler::toggle_camera_lock() -> void
+{
+    if (!camera_lock) {
+        camera_panning = false;
+    }
+
+    camera_lock = !camera_lock;
+}
+
+auto input_handler::toggle_camera_mass_center_lock() -> void
+{
+    if (!camera_mass_center_lock) {
+        camera_lock = true;
+        camera_panning = false;
+    }
+
+    camera_mass_center_lock = !camera_mass_center_lock;
+}
+
+auto input_handler::toggle_camera_projection() const -> void
+{
+    camera.projection = camera.projection == CAMERA_PERSPECTIVE ? CAMERA_ORTHOGRAPHIC : CAMERA_PERSPECTIVE;
+}
+
+auto input_handler::move_block_nor() -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_move_block_at(sel_x, sel_y, nor);
+    if (!next) {
+        return;
+    }
+
+    sel_y--;
+    state.update_current_state(*next);
+}
+
+auto input_handler::move_block_wes() -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_move_block_at(sel_x, sel_y, wes);
+    if (!next) {
+        return;
+    }
+
+    sel_x--;
+    state.update_current_state(*next);
+}
+
+auto input_handler::move_block_sou() -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_move_block_at(sel_x, sel_y, sou);
+    if (!next) {
+        return;
+    }
+
+    sel_y++;
+    state.update_current_state(*next);
+}
+
+auto input_handler::move_block_eas() -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_move_block_at(sel_x, sel_y, eas);
+    if (!next) {
+        return;
+    }
+
+    sel_x++;
+    state.update_current_state(*next);
+}
+
+auto input_handler::print_state() const -> void
+{
+    infoln("State: \"{}\"", state.get_current_state().string_repr());
+}
+
+auto input_handler::load_previous_preset() -> void
+{
+    if (editing) {
+        return;
+    }
+
+    const auto handler = [&]
+    {
+        block_add_x = -1;
+        block_add_y = -1;
+        has_block_add_xy = false;
+        state.load_previous_preset();
+    };
+
+    if (state.was_edited()) {
+        ui_commands.emplace(show_yes_no_message{"Switch Preset?", "Edits Will Be Lost.", handler});
+    } else {
+        handler();
+    }
+}
+
+auto input_handler::load_next_preset() -> void
+{
+    if (editing) {
+        return;
+    }
+
+    const auto handler = [&]
+    {
+        block_add_x = -1;
+        block_add_y = -1;
+        has_block_add_xy = false;
+        state.load_next_preset();
+    };
+
+    if (state.was_edited()) {
+        ui_commands.emplace(show_yes_no_message{"Switch Preset?", "Edits Will Be Lost.", handler});
+    } else {
+        handler();
+    }
+}
+
+auto input_handler::goto_starting_state() -> void
+{
+    const auto handler = [&]
+    {
+        state.goto_starting_state();
+        sel_x = 0;
+        sel_y = 0;
+    };
+
+    ui_commands.emplace(show_yes_no_message{"Reset Board?", "This Clears the Move History.", handler});
+}
+
+auto input_handler::populate_graph() const -> void
+{
+    state.populate_graph();
+}
+
+auto input_handler::clear_graph() -> void
+{
+    const auto handler = [&]
+    {
+        state.clear_graph_and_add_current();
+    };
+
+    ui_commands.emplace(show_yes_no_message{"Clear Graph?", "This Clears the Move History.", handler});
+}
+
+auto input_handler::toggle_mark_solutions() -> void
+{
+    mark_solutions = !mark_solutions;
+}
+
+auto input_handler::toggle_connect_solutions() -> void
+{
+    connect_solutions = !connect_solutions;
+}
+
+auto input_handler::toggle_color_by_distance() -> void
+{
+    color_by_distance = !color_by_distance;
+}
+
+auto input_handler::toggle_mark_path() -> void
+{
+    mark_path = !mark_path;
+}
+
+auto input_handler::goto_optimal_next_state() const -> void
+{
+    state.goto_optimal_next_state();
+}
+
+auto input_handler::goto_most_distant_state() const -> void
+{
+    state.goto_most_distant_state();
+}
+
+auto input_handler::goto_closest_target_state() const -> void
+{
+    state.goto_closest_target_state();
+}
+
+auto input_handler::goto_previous_state() const -> void
+{
+    state.goto_previous_state();
+}
+
+auto input_handler::toggle_restricted_movement() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.toggle_restricted();
+
+    if (!editing || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::toggle_target_block() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_toggle_target(sel_x, sel_y);
+
+    if (!editing || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::toggle_wall_block() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_toggle_wall(sel_x, sel_y);
+
+    if (!editing || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::remove_board_column() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_remove_column();
+
+    if (!editing || current.get_width() <= puzzle::MIN_WIDTH || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::add_board_column() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_add_column();
+
+    if (!editing || current.get_width() >= puzzle::MAX_WIDTH || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::remove_board_row() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_remove_row();
+
+    if (!editing || current.get_height() <= puzzle::MIN_HEIGHT || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::add_board_row() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.try_add_row();
+
+    if (!editing || current.get_height() >= puzzle::MAX_HEIGHT || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::toggle_editing() -> void
+{
+    if (editing) {
+        has_block_add_xy = false;
+        block_add_x = -1;
+        block_add_y = -1;
+    }
+
+    editing = !editing;
+}
+
+auto input_handler::clear_goal() const -> void
+{
+    const puzzle& current = state.get_current_state();
+    const std::optional<puzzle>& next = current.clear_goal();
+
+    if (!editing || !next) {
+        return;
+    }
+
+    state.edit_starting_state(*next);
+}
+
+auto input_handler::save_preset() -> void
+{
+    if (!IsKeyDown(KEY_LEFT_CONTROL)) {
+        return;
+    }
+
+    if (const std::optional<std::string>& reason = state.get_current_state().try_get_invalid_reason()) {
+        ui_commands.emplace(show_ok_message{"Can't Save Preset", std::format("Invalid Board: {}.", *reason)});
+    } else {
+        ui_commands.emplace(show_save_preset_window{});
+    }
+}
+
+auto input_handler::register_generic_handler(const std::function<void(input_handler&)>& handler) -> void
+{
+    generic_handlers.push_back({handler});
+}
+
+auto input_handler::register_mouse_pressed_handler(const MouseButton button,
+                                                   const std::function<void(input_handler&)>& handler) -> void
+{
+    mouse_pressed_handlers.push_back({{handler}, button});
+}
+
+auto input_handler::register_mouse_released_handler(const MouseButton button,
+                                                    const std::function<void(input_handler&)>& handler) -> void
+{
+    mouse_released_handlers.push_back({{handler}, button});
+}
+
+auto input_handler::register_key_pressed_handler(const KeyboardKey key,
+                                                 const std::function<void(input_handler&)>& handler) -> void
+{
+    key_pressed_handlers.push_back({{handler}, key});
+}
+
+auto input_handler::register_key_released_handler(const KeyboardKey key,
+                                                  const std::function<void(input_handler&)>& handler) -> void
+{
+    key_released_handlers.push_back({{handler}, key});
+}
+
+auto input_handler::handle_input() -> void
+{
+    if (disable) {
+        return;
+    }
+
+    for (const auto& [handler] : generic_handlers) {
+        handler(*this);
+    }
+
+    for (const mouse_handler& handler : mouse_pressed_handlers) {
+        if (IsMouseButtonPressed(handler.button)) {
+            handler.handler(*this);
+        }
+    }
+
+    for (const mouse_handler& handler : mouse_released_handlers) {
+        if (IsMouseButtonReleased(handler.button)) {
+            handler.handler(*this);
+        }
+    }
+
+    for (const keyboard_handler& handler : key_pressed_handlers) {
+        if (IsKeyPressed(handler.key)) {
+            handler.handler(*this);
+        }
+    }
+
+    for (const keyboard_handler& handler : key_released_handlers) {
+        if (IsKeyReleased(handler.key)) {
+            handler.handler(*this);
+        }
+    }
+}

src/load_save.cpp

@@ -0,0 +1,82 @@
+#include "load_save.hpp"
+
+#include <fstream>
+
+auto parse_preset_file(const std::string& preset_file) -> std::pair<std::vector<puzzle>, std::vector<std::string>>
+{
+    std::fstream file(preset_file, std::ios::in);
+    if (!file) {
+        infoln("Preset file \"{}\" couldn't be opened.", preset_file);
+        return {};
+    }
+
+    std::string line;
+    std::vector<std::string> comment_lines;
+    std::vector<std::string> preset_lines;
+    while (std::getline(file, line)) {
+        if (line.starts_with("S")) {
+            preset_lines.push_back(line);
+        } else if (line.starts_with("#")) {
+            comment_lines.push_back(line);
+        }
+    }
+
+    if (preset_lines.empty() || comment_lines.size() != preset_lines.size()) {
+        infoln("Preset file \"{}\" couldn't be opened.", preset_file);
+        return {};
+    }
+
+    std::vector<puzzle> preset_states;
+    for (const auto& preset : preset_lines) {
+        // Each char is a bit
+        const puzzle& p = puzzle(preset);
+
+        if (const std::optional<std::string>& reason = p.try_get_invalid_reason()) {
+            infoln("Preset file \"{}\" contained invalid presets: {}", preset_file, *reason);
+            return {};
+        }
+        preset_states.emplace_back(p);
+    }
+
+    infoln("Loaded {} presets from \"{}\".", preset_lines.size(), preset_file);
+
+    return {preset_states, comment_lines};
+}
+
+auto append_preset_file(const std::string& preset_file, const std::string& preset_name, const puzzle& p) -> bool
+{
+    infoln(R"(Saving preset "{}" to "{}")", preset_name, preset_file);
+
+    if (p.try_get_invalid_reason()) {
+        return false;
+    }
+
+    std::fstream file(preset_file, std::ios_base::app | std::ios_base::out);
+    if (!file) {
+        infoln("Preset file \"{}\" couldn't be opened.", preset_file);
+        return false;
+    }
+
+    file << "\n# " << preset_name << "\n" << p.string_repr() << std::flush;
+
+    return true;
+}
+
+auto append_preset_file_quiet(const std::string& preset_file,
+                              const std::string& preset_name,
+                              const puzzle& p,
+                              const bool validate) -> bool
+{
+    if (validate && p.try_get_invalid_reason()) {
+        return false;
+    }
+
+    std::fstream file(preset_file, std::ios_base::app | std::ios_base::out);
+    if (!file) {
+        return false;
+    }
+
+    file << "\n# " << preset_name << "\n" << p.string_repr() << std::flush;
+
+    return true;
+}

src/main.cpp

@@ -1,107 +1,406 @@
-#include <mutex>
-#include <raylib.h>
-#include <raymath.h>
-
 #include "config.hpp"
-#include "input.hpp"
-#include "physics.hpp"
+#include "input_handler.hpp"
+#include "cpu_layout_engine.hpp"
 #include "renderer.hpp"
-#include "state.hpp"
+#include "state_manager.hpp"
+#include "user_interface.hpp"
 
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
+#include <chrono>
+#include <mutex>
+#include <GL/glew.h>
+#include <raylib.h>
+#include <filesystem>
+
+#if not defined(_WIN32)
+#include <boost/program_options.hpp>
+namespace po = boost::program_options;
 #endif
 
-// TODO: Click states in the graph to display them in the board
-
-// NOTE: Tracy uses a huge amount of memory. For longer testing disable Tracy.
-
-auto main(int argc, char *argv[]) -> int {
-  std::string preset_file;
-  if (argc != 2) {
-    preset_file = "default.puzzle";
-  } else {
-    preset_file = argv[1];
-  }
-
-  // RayLib window setup
-  SetTraceLogLevel(LOG_ERROR);
-  // SetConfigFlags(FLAG_VSYNC_HINT);
-  SetConfigFlags(FLAG_MSAA_4X_HINT);
-  SetConfigFlags(FLAG_WINDOW_RESIZABLE);
-  SetConfigFlags(FLAG_WINDOW_ALWAYS_RUN);
-  InitWindow(INITIAL_WIDTH * 2, INITIAL_HEIGHT + MENU_HEIGHT, "MassSprings");
-
-  // Game setup
-  ThreadedPhysics physics;
-  StateManager state(physics, preset_file);
-  InputHandler input(state);
-  OrbitCamera3D camera;
-  Renderer renderer(camera, state, input);
-
-  unsigned int ups;
-  std::vector<Vector3> masses; // Read from physics
-
-  // Game loop
-  while (!WindowShouldClose()) {
-#ifdef TRACY
-    FrameMarkStart("MainThread");
-#endif
-
-    // Input update
-    input.HandleInput();
-    state.UpdateGraph(); // Add state added after user input
-
-    // Read positions from physics thread
-#ifdef TRACY
-    FrameMarkStart("MainThreadConsumeLock");
-#endif
-    {
-#ifdef TRACY
-      std::unique_lock<LockableBase(std::mutex)> lock(physics.state.data_mtx);
-#else
-      std::unique_lock<std::mutex> lock(physics.state.data_mtx);
-#endif
-
-      ups = physics.state.ups;
-
-      // Only copy data if any has been produced
-      if (physics.state.data_ready) {
-        masses = physics.state.masses;
-
-        physics.state.data_ready = false;
-        physics.state.data_consumed = true;
-
-        lock.unlock();
-        // Notify the physics thread that data has been consumed
-        physics.state.data_consumed_cnd.notify_all();
-      }
-    }
-#ifdef TRACY
-    FrameMarkEnd("MainThreadConsumeLock");
-#endif
-
-    // Update the camera after the physics, so target lock is smooth
-    std::size_t current_index = state.CurrentMassIndex();
-    if (masses.size() > current_index) {
-      const Mass &current_mass = masses.at(current_index);
-      camera.Update(current_mass.position);
-    }
-
-    // Rendering
-    renderer.UpdateTextureSizes();
-    renderer.DrawMassSprings(masses);
-    renderer.DrawKlotski();
-    renderer.DrawMenu(masses);
-    renderer.DrawTextures(ups);
-#ifdef TRACY
-    FrameMark;
-    FrameMarkEnd("MainThread");
-#endif
-  }
-
-  CloseWindow();
-
-  return 0;
+// Threadpool setup
+#ifdef THREADPOOL
+auto set_pool_thread_name(size_t idx) -> void
+{
+    BS::this_thread::set_os_thread_name(std::format("worker-{}", idx));
 }
+
+BS::thread_pool<> threads(std::thread::hardware_concurrency() - 2, set_pool_thread_name);
+constexpr threadpool thread_pool = &threads;
+#else
+constexpr threadpool thread_pool = std::nullopt;
+#endif
+
+// Argparse defaults
+std::string preset_file = "default.puzzle";
+std::string output_file = "clusters.puzzle";
+int max_blocks = 5;
+int min_moves = 10;
+
+// Puzzle space setup
+int board_width;
+int board_height;
+int goal_x;
+int goal_y;
+bool restricted;
+blockset2 permitted_blocks;
+block target_block;
+std::tuple<u8, u8, u8, u8> target_block_pos_range;
+
+// TODO: Export cluster to graphviz
+// TODO: Fix naming:
+//       - Target: The block that has to leave the board to win
+//       - Goal: The opening in the board for the target
+//       - Puzzle (not board or state): A puzzle configuration (width, height, goal_x, goal_y, restricted, goal)
+//       - Block: A puzzle block (x, y, width, height, target, immovable)
+//       - Puzzle State: A specific puzzle state (width, height, goal_x, goal_y, restricted, goal, blocks)
+//       - Cluster: A graph of puzzle states connected by moves, generated from a specific Puzzle State
+//       - Puzzle Space: A number of Clusters generated from a generic Puzzle
+// TODO: Add state space generation time to debug overlay
+// TODO: Move selection accordingly when undoing moves (need to diff two states and get the moved blocks)
+
+auto ui_mode() -> int
+{
+    // RayLib window setup
+    SetTraceLogLevel(LOG_ERROR);
+    SetConfigFlags(FLAG_VSYNC_HINT);
+    SetConfigFlags(FLAG_MSAA_4X_HINT);
+    SetConfigFlags(FLAG_WINDOW_RESIZABLE);
+    SetConfigFlags(FLAG_WINDOW_ALWAYS_RUN);
+    InitWindow(INITIAL_WIDTH * 2, INITIAL_HEIGHT + MENU_HEIGHT, "MassSprings");
+
+    // GLEW setup
+    glewExperimental = GL_TRUE;
+    const GLenum glew_err = glewInit();
+    if (glew_err != GLEW_OK) {
+        TraceLog(LOG_FATAL, "Failed to initialize GLEW: %s", glewGetErrorString(glew_err));
+    }
+
+    // Game setup
+    cpu_layout_engine physics(thread_pool);
+    state_manager state(physics, preset_file);
+    orbit_camera camera;
+    input_handler input(state, camera);
+    user_interface gui(input, state, camera);
+    renderer renderer(camera, state, input, gui);
+
+    std::chrono::time_point last = std::chrono::high_resolution_clock::now();
+    std::chrono::duration<double> fps_accumulator(0);
+    int loop_iterations = 0;
+
+    int fps = 0;
+    int ups = 0;                 // Read from physics
+    Vector3 mass_center;         // Read from physics
+    std::vector<Vector3> masses; // Read from physics
+    size_t mass_count = 0;
+    size_t spring_count = 0;
+
+    // Game loop
+    while (!WindowShouldClose()) {
+        #ifdef TRACY
+        FrameMarkStart("MainThread");
+        #endif
+
+        // Time tracking
+        std::chrono::time_point now = std::chrono::high_resolution_clock::now();
+        std::chrono::duration<double> delta_time = now - last;
+        fps_accumulator += delta_time;
+        last = now;
+
+        // Input update
+        input.handle_input();
+
+        // Read positions from physics thread
+        #ifdef TRACY
+        FrameMarkStart("MainThreadConsumeLock");
+        #endif
+        {
+            #ifdef TRACY
+            std::unique_lock<LockableBase(std::mutex)> lock(physics.state.data_mtx);
+            #else
+            std::unique_lock<std::mutex> lock(physics.state.data_mtx);
+            #endif
+
+            ups = physics.state.ups;
+            mass_center = physics.state.mass_center;
+            mass_count = physics.state.mass_count;
+            spring_count = physics.state.spring_count;
+
+            // Only copy data if any has been produced
+            if (physics.state.data_ready) {
+                masses = physics.state.masses;
+
+                physics.state.data_ready = false;
+                physics.state.data_consumed = true;
+
+                lock.unlock();
+                // Notify the physics thread that data has been consumed
+                physics.state.data_consumed_cnd.notify_all();
+            }
+        }
+        #ifdef TRACY
+        FrameMarkEnd("MainThreadConsumeLock");
+        #endif
+
+        // Update the camera after the physics, so target lock is smooth
+        size_t current_index = state.get_current_index();
+        if (masses.size() > current_index) {
+            const Vector3& current_mass = masses[current_index];
+            camera.update(current_mass, mass_center, input.camera_lock, input.camera_mass_center_lock);
+        }
+
+        // Rendering
+        renderer.render(masses, fps, ups, mass_count, spring_count);
+
+        if (fps_accumulator.count() > 1.0) {
+            // Update each second
+            fps = loop_iterations;
+            loop_iterations = 0;
+            fps_accumulator = std::chrono::duration<double>(0);
+        }
+        ++loop_iterations;
+
+        #ifdef TRACY
+        FrameMark;
+        FrameMarkEnd("MainThread");
+        #endif
+    }
+
+    CloseWindow();
+
+    return 0;
+}
+
+auto rush_hour_puzzle_space() -> void
+{
+    board_width = 6;
+    board_height = 6;
+    goal_x = 4;
+    goal_y = 2;
+    restricted = true;
+    permitted_blocks = {
+        block(0, 0, 2, 1, false, false),
+        block(0, 0, 3, 1, false, false),
+        block(0, 0, 1, 2, false, false),
+        block(0, 0, 1, 3, false, false)
+    };
+    target_block = block(0, 0, 2, 1, true, false);
+    target_block_pos_range = {0, goal_y, board_width - target_block.get_width(), goal_y};
+}
+
+auto klotski_puzzle_space() -> void
+{
+    board_width = 4;
+    board_height = 5;
+    goal_x = 1;
+    goal_y = 3;
+    restricted = false;
+    permitted_blocks = {
+        block(0, 0, 1, 1, false, false),
+        block(0, 0, 1, 2, false, false),
+        block(0, 0, 2, 1, false, false),
+    };
+    target_block = block(0, 0, 2, 2, true, false);
+    target_block_pos_range = {
+        0,
+        0,
+        board_width - target_block.get_width(),
+        board_height - target_block.get_height(),
+    };
+}
+
+auto puzzle_space() -> int
+{
+    // We don't only pick max_blocks out of n (with duplicates), but also 1 out of n, 2, 3, ... max_blocks-1 out of n
+    int upper_set_count = 0;
+    for (int i = 1; i <= max_blocks; ++i) {
+        upper_set_count += binom(permitted_blocks.size() + i - 1, i);
+    }
+
+    infoln("Exploring puzzle space:");
+    infoln("- Size:       {}x{}", board_width, board_height);
+    infoln("- Goal:       {},{}", goal_x, goal_y);
+    infoln("- Restricted: {}", restricted);
+    infoln("- Max Blocks: {}", max_blocks);
+    infoln("- Min Moves:  {}", min_moves);
+    infoln("- Target:     {}x{}", target_block.get_width(), target_block.get_height());
+    infoln("- Max Sets:   {}", upper_set_count);
+    infoln("- Permitted block sizes:");
+    std::cout << "         ";
+    for (const block b : permitted_blocks) {
+        std::cout << std::format(" {}x{},", b.get_width(), b.get_height());
+    }
+    std::cout << std::endl;
+
+    const puzzle p = puzzle(board_width, board_height, goal_x, goal_y, restricted, true);
+
+    STARTTIME;
+    const puzzleset result = p.explore_puzzle_space(
+        permitted_blocks,
+        target_block,
+        target_block_pos_range,
+        max_blocks,
+        min_moves,
+        thread_pool);
+    ENDTIME(std::format("Found {} different clusters", result.size()), std::chrono::seconds, "s");
+
+    // TODO: The exported clusters are the numerically smallest state of the cluster.
+    //       Not the state with the longest path.
+
+    infoln("Sorting clusters...");
+    std::vector<puzzle> result_sorted{result.begin(), result.end()};
+    std::ranges::sort(result_sorted, std::ranges::greater{});
+
+    infoln("Saving clusters...");
+    size_t i = 0;
+    size_t success = 0;
+    std::filesystem::remove(output_file);
+    for (const puzzle& _p : result_sorted) {
+        if (append_preset_file_quiet(output_file, std::format("Cluster {}", i), _p, true)) {
+            ++success;
+        }
+        ++i;
+    }
+    if (success != result_sorted.size()) {
+        warnln("Saved {} of {} clusters", success, result_sorted.size());
+    } else {
+        infoln("Saved {} of {} clusters", success, result_sorted.size());
+    }
+
+    return 0;
+}
+
+enum class runmode
+{
+    USER_INTERFACE,
+    RUSH_HOUR_PUZZLE_SPACE,
+    KLOTSKI_PUZZLE_SPACE,
+    EXIT,
+};
+
+auto argparse(const int argc, char* argv[]) -> runmode
+{
+    #if not defined(_WIN32)
+    po::options_description desc("Allowed options");
+    desc.add_options()                                                                                         //
+        ("help", "produce help message")                                                                       //
+        ("presets", po::value<std::string>()->default_value(preset_file), "load presets from file")            //
+        ("output", po::value<std::string>()->default_value(output_file), "output file for generated clusters") //
+        ("space", po::value<std::string>()->value_name("rh|klotski"), "generate puzzle space with ruleset")    //
+        // ("w", po::value<int>()->default_value(board_width)->value_name("[3, 8]"), "board width")                  //
+        // ("h", po::value<int>()->default_value(board_height)->value_name("[3, 8"), "board height")                 //
+        // ("gx", po::value<int>()->default_value(goal_x)->value_name("[0, w-1]"), "board goal horizontal position") //
+        // ("gy", po::value<int>()->default_value(goal_y)->value_name("[0, h-1]"), "board goal vertical position")   //
+        // ("free", "allow free block movement")                                                                     //
+        ("blocks",
+         po::value<int>()->default_value(max_blocks)->value_name("[1, 15]"),
+         "block limit for puzzle space generation") //
+        ("moves",
+         po::value<int>()->default_value(min_moves),
+         "only save puzzles with at least this many required moves") //
+        ;
+
+    po::positional_options_description positional;
+    positional.add("presets", -1);
+
+    po::variables_map vm;
+    po::store(po::command_line_parser(argc, argv).options(desc).positional(positional).run(), vm);
+    po::notify(vm);
+
+    if (vm.contains("help")) {
+        std::cout << desc << std::endl;
+        return runmode::EXIT;
+    }
+    if (vm.contains("output")) {
+        output_file = vm["output"].as<std::string>();
+    }
+
+    // if (vm.contains("w")) {
+    //     board_width = vm["w"].as<int>();
+    //     board_width = std::max(static_cast<int>(puzzle::MIN_WIDTH),
+    //                            std::min(board_width, static_cast<int>(puzzle::MAX_WIDTH)));
+    // }
+    // if (vm.contains("h")) {
+    //     board_height = vm["h"].as<int>();
+    //     board_height = std::max(static_cast<int>(puzzle::MIN_HEIGHT),
+    //                             std::min(board_height, static_cast<int>(puzzle::MAX_HEIGHT)));
+    // }
+    // if (vm.contains("gx")) {
+    //     goal_x = vm["gx"].as<int>();
+    //     goal_x = std::max(0, std::min(goal_x, static_cast<int>(puzzle::MAX_WIDTH) - 1));
+    // }
+    // if (vm.contains("gy")) {
+    //     goal_y = vm["gy"].as<int>();
+    //     goal_y = std::max(0, std::min(goal_y, static_cast<int>(puzzle::MAX_HEIGHT) - 1));
+    // }
+    // if (vm.contains("free")) {
+    //     restricted = false;
+    // }
+
+    if (vm.contains("blocks")) {
+        max_blocks = vm["blocks"].as<int>();
+        max_blocks = std::max(1, std::min(max_blocks, static_cast<int>(puzzle::MAX_BLOCKS)));
+    }
+    if (vm.contains("moves")) {
+        min_moves = vm["moves"].as<int>();
+        min_moves = std::max(0, min_moves);
+    }
+    if (vm.contains("space")) {
+        const std::string ruleset = vm["space"].as<std::string>();
+        if (ruleset == "rh") {
+            return runmode::RUSH_HOUR_PUZZLE_SPACE;
+        }
+        if (ruleset == "klotski") {
+            return runmode::KLOTSKI_PUZZLE_SPACE;
+        }
+    }
+    if (vm.contains("presets")) {
+        preset_file = vm["presets"].as<std::string>();
+    }
+    #endif
+
+    return runmode::USER_INTERFACE;
+}
+
+auto main(const int argc, char* argv[]) -> int
+{
+    #ifdef BACKWARD
+    infoln("Backward stack-traces enabled.");
+    #else
+    infoln("Backward stack-traces disabled.");
+    #endif
+
+    #ifdef TRACY
+    infoln("Tracy adapter enabled.");
+    #else
+    infoln("Tracy adapter disabled.");
+    #endif
+
+    infoln("Using background thread for physics.");
+    infoln("Using linear octree + Barnes-Hut for graph layout.");
+
+    #ifdef ASYNC_OCTREE
+    infoln("Using asynchronous octree build.");
+    #else
+    infoln("Using synchronous octree build.");
+    #endif
+
+    #ifdef THREADPOOL
+    infoln("Additional thread-pool enabled ({} threads).", threads.get_thread_count());
+    #else
+    infoln("Additional thread-pool disabled.");
+    #endif
+
+    switch (argparse(argc, argv)) {
+    case runmode::USER_INTERFACE:
+        return ui_mode();
+    case runmode::RUSH_HOUR_PUZZLE_SPACE:
+        rush_hour_puzzle_space();
+        return puzzle_space();
+    case runmode::KLOTSKI_PUZZLE_SPACE:
+        klotski_puzzle_space();
+        return puzzle_space();
+    case runmode::EXIT:
+        return 0;
+    };
+
+    return 1;
+}

src/octree.cpp

@@ -1,178 +1,335 @@
 #include "octree.hpp"
 #include "config.hpp"
-#include "util.hpp"
 
-#include <iostream>
+#include <cfloat>
 #include <raymath.h>
 
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
+auto octree::clear() -> void
+{
+    nodes.clear();
+}
 
-auto OctreeNode::ChildCount() const -> int {
-  int child_count = 0;
-  for (int child : children) {
-    if (child != -1) {
-      ++child_count;
+auto octree::reserve(const size_t count) -> void
+{
+    nodes.reserve(count);
+}
+
+auto octree::empty() const -> bool
+{
+    return nodes.empty();
+}
+
+auto octree::root() const -> const node&
+{
+    return nodes[0];
+}
+
+// Replaced the 50 line recursive octree insertion with this morton bitch to gain 5 UPS, FML
+auto octree::build_octree_morton(octree& t,
+                                 const std::vector<Vector3>& positions,
+                                 const std::optional<BS::thread_pool<>*>& thread_pool) -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    t.clear();
+    if (positions.empty()) {
+        return;
     }
-  }
-  return child_count;
-}
 
-auto Octree::CreateNode(const Vector3 &box_min, const Vector3 &box_max) -> int {
-  OctreeNode node;
-  node.box_min = box_min;
-  node.box_max = box_max;
-  nodes.push_back(node);
-
-  return nodes.size() - 1;
-}
-
-auto Octree::GetOctant(int node_idx, const Vector3 &pos) -> int {
-  OctreeNode &node = nodes[node_idx];
-  Vector3 center = Vector3((node.box_min.x + node.box_max.x) / 2.0,
-                           (node.box_min.y + node.box_max.y) / 2.0,
-                           (node.box_min.z + node.box_max.z) / 2.0);
-
-  // The octant is encoded as a 3-bit integer "zyx". The node area is split
-  // along all 3 axes, if a position is right of an axis, this bit is set to 1.
-  // If a position is right of the x-axis and y-axis and left of the z-axis, the
-  // encoded octant is "011".
-  int octant = 0;
-  if (pos.x >= center.x) {
-    octant |= 1;
-  }
-  if (pos.y >= center.y) {
-    octant |= 2;
-  }
-  if (pos.z >= center.z) {
-    octant |= 4;
-  }
-
-  return octant;
-}
-
-auto Octree::GetChildBounds(int node_idx, int octant)
-    -> std::pair<Vector3, Vector3> {
-  OctreeNode &node = nodes[node_idx];
-  Vector3 center = Vector3((node.box_min.x + node.box_max.x) / 2.0,
-                           (node.box_min.y + node.box_max.y) / 2.0,
-                           (node.box_min.z + node.box_max.z) / 2.0);
-
-  Vector3 min = Vector3Zero();
-  Vector3 max = Vector3Zero();
-
-  // If (octant & 1), the octant is to the right of the node region's x-axis
-  // (see GetOctant). This means the left bound is the x-axis and the right
-  // bound the node's region max.
-  min.x = (octant & 1) ? center.x : node.box_min.x;
-  max.x = (octant & 1) ? node.box_max.x : center.x;
-  min.y = (octant & 2) ? center.y : node.box_min.y;
-  max.y = (octant & 2) ? node.box_max.y : center.y;
-  min.z = (octant & 4) ? center.z : node.box_min.z;
-  max.z = (octant & 4) ? node.box_max.z : center.z;
-
-  return std::make_pair(min, max);
-}
-
-auto Octree::Insert(int node_idx, int mass_id, const Vector3 &pos, float mass)
-    -> void {
-  // NOTE: Do not store a nodes[node_idx] reference beforehand as the nodes
-  //       vector might reallocate during this function
-
-  if (nodes[node_idx].leaf && nodes[node_idx].mass_id == -1) {
-    // We can place the particle in the empty leaf
-    nodes[node_idx].mass_id = mass_id;
-    nodes[node_idx].mass_center = pos;
-    nodes[node_idx].mass_total = mass;
-    return;
-  }
-
-  if (nodes[node_idx].leaf) {
-    // The leaf is occupied, we need to subdivide
-    int existing_id = nodes[node_idx].mass_id;
-    Vector3 existing_pos = nodes[node_idx].mass_center;
-    float existing_mass = nodes[node_idx].mass_total;
-    nodes[node_idx].mass_id = -1;
-    nodes[node_idx].leaf = false;
-    nodes[node_idx].mass_total = 0.0;
-
-    // Re-insert the existing mass into a new empty leaf (see above)
-    int oct = GetOctant(node_idx, existing_pos);
-    if (nodes[node_idx].children[oct] == -1) {
-      auto [min, max] = GetChildBounds(node_idx, oct);
-      nodes[node_idx].children[oct] = CreateNode(min, max);
+    // Compute bounding box around all masses
+    Vector3 root_min{FLT_MAX, FLT_MAX, FLT_MAX};
+    Vector3 root_max{-FLT_MAX, -FLT_MAX, -FLT_MAX};
+    for (const auto& [x, y, z] : positions) {
+        root_min.x = std::min(root_min.x, x);
+        root_max.x = std::max(root_max.x, x);
+        root_min.y = std::min(root_min.y, y);
+        root_max.y = std::max(root_max.y, y);
+        root_min.z = std::min(root_min.z, z);
+        root_max.z = std::max(root_max.z, z);
     }
-    Insert(nodes[node_idx].children[oct], existing_id, existing_pos,
-           existing_mass);
-  }
 
-  // Insert the new mass
-  int oct = GetOctant(node_idx, pos);
-  if (nodes[node_idx].children[oct] == -1) {
-    auto [min, max] = GetChildBounds(node_idx, oct);
-    nodes[node_idx].children[oct] = CreateNode(min, max);
-  }
-  Insert(nodes[node_idx].children[oct], mass_id, pos, mass);
+    constexpr float pad = 1.0f;
+    root_min = Vector3Subtract(root_min, Vector3Scale(Vector3One(), pad));
+    root_max = Vector3Add(root_max, Vector3Scale(Vector3One(), pad));
 
-  // Update the center of mass
-  float new_mass = nodes[node_idx].mass_total + mass;
-  nodes[node_idx].mass_center.x =
-      (nodes[node_idx].mass_center.x * nodes[node_idx].mass_total + pos.x) /
-      new_mass;
-  nodes[node_idx].mass_center.y =
-      (nodes[node_idx].mass_center.y * nodes[node_idx].mass_total + pos.y) /
-      new_mass;
-  nodes[node_idx].mass_center.z =
-      (nodes[node_idx].mass_center.z * nodes[node_idx].mass_total + pos.z) /
-      new_mass;
-  nodes[node_idx].mass_total = new_mass;
-}
+    const float max_extent = std::max({root_max.x - root_min.x, root_max.y - root_min.y, root_max.z - root_min.z});
+    root_max = Vector3Add(root_min, Vector3Scale(Vector3One(), max_extent));
 
-auto Octree::CalculateForce(int node_idx, const Vector3 &pos) const -> Vector3 {
-  if (node_idx < 0) {
-    return Vector3Zero();
-  }
+    const float root_extent = root_max.x - root_min.x; // cubic
 
-  const OctreeNode &node = nodes[node_idx];
-  if (std::abs(node.mass_total) <= 0.001f) {
-    return Vector3Zero();
-  }
+    // Container for building the particle list before sorting by morton code
+    struct sort_node
+    {
+        u64 code;
+        u32 id;
+        Vector3 pos;
+    };
 
-  Vector3 diff = Vector3Subtract(pos, node.mass_center);
-  float dist_sq = diff.x * diff.x + diff.y * diff.y + diff.z * diff.z;
+    // Calculate morton code for each node
+    std::vector<sort_node> sort_container;
+    sort_container.resize(positions.size());
 
-  // Softening
-  dist_sq += SOFTENING;
+    const auto calculate_morton = [&](const u32 i)
+    {
+        sort_container[i] = {pos_to_morton(positions[i], root_min, root_max), i, positions[i]};
+    };
 
-  float size = node.box_max.x - node.box_min.x;
-
-  // Barnes-Hut
-  if (node.leaf || (size * size / dist_sq) < (THETA * THETA)) {
-    float dist = std::sqrt(dist_sq);
-    float force_mag = BH_FORCE * node.mass_total / dist_sq;
-
-    return Vector3Scale(diff, force_mag / dist);
-  }
-
-  // Collect child forces
-  Vector3 force = Vector3Zero();
-  for (int i = 0; i < 8; ++i) {
-    if (node.children[i] >= 0) {
-      Vector3 child_force = CalculateForce(node.children[i], pos);
-
-      force = Vector3Add(force, child_force);
+    if (thread_pool) {
+        (*thread_pool)->submit_loop(0, positions.size(), calculate_morton, SMALL_TASK_BLOCK_SIZE).wait();
+    } else {
+        for (u32 i = 0; i < positions.size(); ++i) {
+            calculate_morton(i);
+        }
     }
-  }
 
-  return force;
+    // Sort the list by morton codes. Because positions close to each other have similar morten codes,
+    // this provides us with "spatial locality" in the datastructure.
+    auto sort_by_code = [&]()
+    {
+        std::ranges::sort(sort_container,
+                          [](const sort_node& a, const sort_node& b)
+                          {
+                              if (a.code != b.code) {
+                                  return a.code < b.code;
+                              }
+                              return a.id < b.id;
+                          });
+    };
+
+    sort_by_code();
+
+    // Resolve duplicates by jittering the later one deterministically and re-encoding.
+    for (int seed = 0; seed < 8; ++seed) {
+        bool had_dupes = false;
+
+        for (size_t i = 1; i < sort_container.size(); ++i) {
+            // Because elements are spatially ordered after sorting, we can scan for duplicates in O(n)
+            if (sort_container[i].code == sort_container[i - 1].code) {
+                had_dupes = true;
+                sort_container[i].pos = jitter_pos(sort_container[i].pos,
+                                                   sort_container[i].id + seed * 0x9e3779b9u,
+                                                   root_min,
+                                                   root_max,
+                                                   root_extent);
+                sort_container[i].code = pos_to_morton(sort_container[i].pos, root_min, root_max);
+            }
+        }
+
+        if (!had_dupes) {
+            break;
+        }
+        sort_by_code();
+    }
+
+    // Sanity check
+    for (size_t i = 1; i < sort_container.size(); ++i) {
+        if (sort_container[i].code == sort_container[i - 1].code) {
+            throw std::runtime_error("Duplicates remain after jittering");
+        }
+    }
+
+    std::vector<std::vector<node>> tree_levels;
+    tree_levels.assign(MAX_DEPTH + 1, {});
+
+    // Leaves at MAX_DEPTH: 1 particle per leaf in morton order (close particles close together)
+    auto& leafs = tree_levels[MAX_DEPTH];
+    leafs.reserve(sort_container.size());
+    const float leaf_size = root_extent / static_cast<float>(1u << MAX_DEPTH);
+    for (const auto& [code, id, pos] : sort_container) {
+        node leaf;
+        leaf.leaf = true;
+        leaf.mass_id = static_cast<int>(id);
+        leaf.depth = MAX_DEPTH;
+        leaf.size = leaf_size;
+        leaf.mass_total = MASS;
+        leaf.mass_center = pos; // force uses mass_center instead of jittered position
+        leaf.children.fill(-1);
+        leafs.push_back(leaf);
+    }
+
+    // We now have to group the particles (currently we have only sorted the leaves),
+    // but upwards subdivisions have to be created.
+    // For grouping, store a nodes local index in its level.
+    struct leaf
+    {
+        u64 leaf_code;
+        int depth;
+        int level_index;
+    };
+
+    std::vector<leaf> leaves;
+    leaves.reserve(tree_levels[MAX_DEPTH].size());
+    for (int i = 0; i < static_cast<int>(tree_levels[MAX_DEPTH].size()); ++i) {
+        leaves.emplace_back(sort_container[static_cast<size_t>(i)].code, MAX_DEPTH, i);
+    }
+
+    // Build internal levels from MAX_DEPTH-1 to 0
+    for (int current_depth = MAX_DEPTH - 1; current_depth >= 0; --current_depth) {
+        auto& current_level = tree_levels[current_depth];
+        current_level.clear();
+
+        std::vector<leaf> next_refs;
+        next_refs.reserve(leaves.size());
+
+        const float parent_size = root_extent / static_cast<float>(1u << current_depth);
+
+        size_t i = 0;
+        while (i < leaves.size()) {
+            const u64 key = path_to_ancestor(leaves[i].leaf_code, MAX_DEPTH, current_depth);
+
+            size_t j = i + 1;
+            while (j < leaves.size() && path_to_ancestor(leaves[j].leaf_code, MAX_DEPTH, current_depth) == key) {
+                ++j;
+            }
+
+            const size_t group_size = j - i;
+
+            // Unary compression: just carry the child ref upward unchanged.
+            if (group_size == 1) {
+                next_refs.push_back(leaves[i]);
+                i = j;
+                continue;
+            }
+
+            node parent;
+            parent.leaf = false;
+            parent.mass_id = -1;
+            parent.depth = current_depth;
+            parent.size = parent_size;
+            parent.children.fill(-1);
+
+            float mass_total = 0.0f;
+            Vector3 mass_center_acc = Vector3Zero();
+
+            for (size_t k = i; k < j; ++k) {
+                const int child_depth = leaves[k].depth;
+                const int child_local = leaves[k].level_index;
+
+                // Read the child from its actual stored level.
+                const node& child = tree_levels[child_depth][child_local];
+
+                // Which octant of this parent does it belong to?
+                // Octant comes from the NEXT level after current_depth,
+                // but the child might skip levels due to compression.
+                // We must use the child's first level under the parent (current_depth+1).
+                const int oct = octant_at_level(leaves[k].leaf_code, current_depth + 1, MAX_DEPTH);
+
+                // Store global child reference: we only have an int slot, so we need a single index space.
+                parent.children[oct] = (child_depth << 24) | (child_local & 0x00FFFFFF);
+
+                mass_total += child.mass_total;
+                mass_center_acc = Vector3Add(mass_center_acc, Vector3Scale(child.mass_center, child.mass_total));
+            }
+
+            parent.mass_total = mass_total;
+            parent.mass_center = (mass_total > 0.0f) ? Vector3Scale(mass_center_acc, 1.0f / mass_total) : Vector3Zero();
+
+            const int parent_local = static_cast<int>(current_level.size());
+            current_level.push_back(parent);
+
+            next_refs.push_back({leaves[i].leaf_code, current_depth, parent_local});
+            i = j;
+        }
+
+        leaves.swap(next_refs);
+    }
+
+    // Flatten levels and fix child indices from local->global.
+    // All depth 0 nodes come first, then depth 1, last depth MAX_DEPTH.
+    std::vector<int> offsets(tree_levels.size(), 0);
+    int total = 0;
+    for (int d = 0; d <= MAX_DEPTH; ++d) {
+        offsets[d] = total;
+        total += static_cast<int>(tree_levels[d].size());
+    }
+
+    t.nodes.clear();
+    t.nodes.reserve(total);
+    for (int d = 0; d <= MAX_DEPTH; ++d) {
+        t.nodes.insert(t.nodes.end(), tree_levels[d].begin(), tree_levels[d].end());
+    }
+
+    // Fix child indices: convert local index in levels[d+1] to global index in t.nodes
+    for (int d = 0; d <= MAX_DEPTH; ++d) {
+        const int base = offsets[d];
+        for (int i2 = 0; i2 < static_cast<int>(tree_levels[d].size()); ++i2) {
+            node& n = t.nodes[base + i2];
+            if (!n.leaf) {
+                for (int c = 0; c < 8; ++c) {
+                    int packed = n.children[c];
+                    if (packed >= 0) {
+                        const int child_depth = (packed >> 24) & 0xFF;
+                        const int child_local = packed & 0x00FFFFFF;
+                        n.children[c] = offsets[child_depth] + child_local;
+                    }
+                }
+            }
+        }
+    }
+
+    // const size_t _leaves = tree_levels[MAX_DEPTH].size();
+    // const size_t _total = t.nodes.size();
+    // const size_t _internal = _total - _leaves;
+    // traceln("Morton octree nodes: {}, leaves: {}, ratio: {:.3} children per internal node.",
+    //         _total,
+    //         _leaves,
+    //         static_cast<float>(_total - 1) / _internal);
 }
 
-auto Octree::Print() const -> void {
-  std::cout << "Octree Start ===========================" << std::endl;
-  for (const auto &node : nodes) {
-    std::cout << "Center: " << node.mass_center << ", Mass: " << node.mass_total
-              << ", Direct Children: " << node.ChildCount() << std::endl;
-  }
-}
+auto octree::calculate_force_morton(const int node_idx, const Vector3& pos, const int self_id) const -> Vector3
+{
+    if (node_idx < 0) {
+        return Vector3Zero();
+    }
+
+    // Force accumulator
+    float fx = 0.0f;
+    float fy = 0.0f;
+    float fz = 0.0f;
+
+    std::vector<int> stack;
+    stack.reserve(512);
+    stack.push_back(node_idx);
+
+    constexpr float theta2 = THETA * THETA;
+
+    while (!stack.empty()) {
+        const int idx = stack.back();
+        stack.pop_back();
+
+        const node& n = nodes[idx];
+
+        // No self-force for single-particle leafs
+        if (n.leaf && n.mass_id == self_id) {
+            continue;
+        }
+
+        const float dx = pos.x - n.mass_center.x;
+        const float dy = pos.y - n.mass_center.y;
+        const float dz = pos.z - n.mass_center.z;
+
+        const float dist_sq = dx * dx + dy * dy + dz * dz + SOFTENING;
+
+        // Barnes–Hut criterion
+        if (n.leaf || ((n.size * n.size) / dist_sq) < theta2) {
+            const float inv_dist = 1.0f / std::sqrt(dist_sq);
+            const float force_mag = (BH_FORCE * n.mass_total) / dist_sq; // ~ 1/r^2
+            const float s = force_mag * inv_dist;                        // scale by 1/r to get vector
+            fx += dx * s;
+            fy += dy * s;
+            fz += dz * s;
+            continue;
+        }
+
+        for (int c = 0; c < 8; ++c) {
+            const int child = n.children[c];
+            if (child >= 0) {
+                stack.push_back(child);
+            }
+        }
+    }
+
+    return Vector3{fx, fy, fz};
+}

src/orbit_camera.cpp

@@ -0,0 +1,79 @@
+#include "orbit_camera.hpp"
+#include "config.hpp"
+
+#include <raylib.h>
+#include <raymath.h>
+
+auto orbit_camera::rotate(const Vector2 last_mouse, const Vector2 mouse) -> void
+{
+    const auto [dx, dy] = Vector2Subtract(mouse, last_mouse);
+
+    angle_x -= dx * ROT_SPEED / 200.0f;
+    angle_y += dy * ROT_SPEED / 200.0f;
+
+    angle_y = Clamp(angle_y, -1.5, 1.5); // Prevent flipping
+}
+
+auto orbit_camera::pan(const Vector2 last_mouse, const Vector2 mouse) -> void
+{
+    const auto [dx, dy] = Vector2Subtract(mouse, last_mouse);
+
+    float speed;
+    if (IsKeyDown(KEY_LEFT_SHIFT)) {
+        speed = distance * PAN_SPEED / 1000.0f * PAN_MULTIPLIER;
+    } else {
+        speed = distance * PAN_SPEED / 1000.0f;
+    }
+
+    // The panning needs to happen in camera coordinates, otherwise rotating the
+    // camera breaks it
+    const Vector3 forward = Vector3Normalize(Vector3Subtract(camera.target, camera.position));
+    const Vector3 right = Vector3Normalize(Vector3CrossProduct(forward, camera.up));
+    const Vector3 up = Vector3Normalize(Vector3CrossProduct(right, forward));
+
+    const Vector3 offset = Vector3Add(Vector3Scale(right, -dx * speed), Vector3Scale(up, dy * speed));
+
+    target = Vector3Add(target, offset);
+}
+
+auto orbit_camera::update(const Vector3& current_target,
+                          const Vector3& mass_center,
+                          const bool lock,
+                          const bool mass_center_lock) -> void
+{
+    if (lock) {
+        if (mass_center_lock) {
+            target = Vector3MoveTowards(target,
+                                        mass_center,
+                                        CAMERA_SMOOTH_SPEED * GetFrameTime() * Vector3Length(
+                                            Vector3Subtract(target, mass_center)));
+        } else {
+            target = Vector3MoveTowards(target,
+                                        current_target,
+                                        CAMERA_SMOOTH_SPEED * GetFrameTime() * Vector3Length(
+                                            Vector3Subtract(target, current_target)));
+        }
+    }
+
+    distance = Clamp(distance, MIN_CAMERA_DISTANCE, MAX_CAMERA_DISTANCE);
+    float actual_distance = distance;
+    if (projection == CAMERA_ORTHOGRAPHIC) {
+        actual_distance = MAX_CAMERA_DISTANCE;
+    }
+
+    // Spherical coordinates
+    const float x = cos(angle_y) * sin(angle_x) * actual_distance;
+    const float y = sin(angle_y) * actual_distance;
+    const float z = cos(angle_y) * cos(angle_x) * actual_distance;
+
+    if (projection == CAMERA_ORTHOGRAPHIC) {
+        fov = Clamp(fov, MIN_FOV, MAX_ORTHO_FOV);
+    } else {
+        fov = Clamp(fov, MIN_FOV, MAX_PERSP_FOV);
+    }
+
+    camera.position = Vector3Add(target, Vector3(x, y, z));
+    camera.target = target;
+    camera.fovy = fov;
+    camera.projection = projection;
+}

src/physics.cpp

@@ -1,353 +0,0 @@
-#include "physics.hpp"
-#include "config.hpp"
-
-#include <algorithm>
-#include <cfloat>
-#include <chrono>
-#include <cstddef>
-#include <raylib.h>
-#include <raymath.h>
-#include <utility>
-#include <vector>
-
-#ifdef THREADPOOL
-#define BS_THREAD_POOL_NATIVE_EXTENSIONS
-#include <BS_thread_pool.hpp>
-#endif
-
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-auto Mass::ClearForce() -> void { force = Vector3Zero(); }
-
-auto Mass::CalculateVelocity(const float delta_time) -> void {
-  Vector3 acceleration;
-  Vector3 temp;
-
-  acceleration = Vector3Scale(force, 1.0 / MASS);
-  temp = Vector3Scale(acceleration, delta_time);
-  velocity = Vector3Add(velocity, temp);
-}
-
-auto Mass::CalculatePosition(const float delta_time) -> void {
-  previous_position = position;
-
-  Vector3 temp;
-
-  temp = Vector3Scale(velocity, delta_time);
-  position = Vector3Add(position, temp);
-}
-
-auto Mass::VerletUpdate(const float delta_time) -> void {
-  Vector3 acceleration = Vector3Scale(force, 1.0 / MASS);
-  Vector3 temp_position = position;
-
-  Vector3 displacement = Vector3Subtract(position, previous_position);
-  Vector3 accel_term = Vector3Scale(acceleration, delta_time * delta_time);
-
-  // Minimal dampening
-  displacement = Vector3Scale(displacement, 1.0 - VERLET_DAMPENING);
-
-  position = Vector3Add(Vector3Add(position, displacement), accel_term);
-  previous_position = temp_position;
-}
-
-auto Spring::CalculateSpringForce(Mass &_mass_a, Mass &_mass_b) const -> void {
-  Vector3 delta_position = Vector3Subtract(_mass_a.position, _mass_b.position);
-  float current_length = Vector3Length(delta_position);
-  float inv_current_length = 1.0 / current_length;
-  Vector3 delta_velocity = Vector3Subtract(_mass_a.velocity, _mass_b.velocity);
-
-  float hooke = SPRING_CONSTANT * (current_length - REST_LENGTH);
-  float dampening = DAMPENING_CONSTANT *
-                    Vector3DotProduct(delta_velocity, delta_position) *
-                    inv_current_length;
-
-  Vector3 force_a =
-      Vector3Scale(delta_position, -(hooke + dampening) * inv_current_length);
-  Vector3 force_b = Vector3Scale(force_a, -1.0);
-
-  _mass_a.force = Vector3Add(_mass_a.force, force_a);
-  _mass_b.force = Vector3Add(_mass_b.force, force_b);
-}
-
-auto MassSpringSystem::AddMass() -> void { masses.emplace_back(Vector3Zero()); }
-
-auto MassSpringSystem::AddSpring(int a, int b) -> void {
-  Mass &mass_a = masses.at(a);
-  Mass &mass_b = masses.at(b);
-
-  Vector3 position = mass_a.position;
-  Vector3 offset = Vector3(static_cast<float>(GetRandomValue(-100, 100)),
-                           static_cast<float>(GetRandomValue(-100, 100)),
-                           static_cast<float>(GetRandomValue(-100, 100)));
-  offset = Vector3Scale(Vector3Normalize(offset), REST_LENGTH);
-
-  if (mass_b.position == Vector3Zero()) {
-    mass_b.position = Vector3Add(position, offset);
-  }
-
-  springs.emplace_back(a, b);
-}
-
-auto MassSpringSystem::Clear() -> void {
-  masses.clear();
-  springs.clear();
-  octree.nodes.clear();
-}
-
-auto MassSpringSystem::ClearForces() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  for (auto &mass : masses) {
-    mass.ClearForce();
-  }
-}
-
-auto MassSpringSystem::CalculateSpringForces() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  for (const auto spring : springs) {
-    Mass &a = masses.at(spring.a);
-    Mass &b = masses.at(spring.b);
-    spring.CalculateSpringForce(a, b);
-  }
-}
-
-#ifdef THREADPOOL
-auto MassSpringSystem::SetThreadName(std::size_t idx) -> void {
-  BS::this_thread::set_os_thread_name(std::format("bh-worker-{}", idx));
-}
-#endif
-
-auto MassSpringSystem::BuildOctree() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  octree.nodes.clear();
-  octree.nodes.reserve(masses.size() * 2);
-
-  // Compute bounding box around all masses
-  Vector3 min = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
-  Vector3 max = Vector3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
-  for (const auto &mass : masses) {
-    min.x = std::min(min.x, mass.position.x);
-    max.x = std::max(max.x, mass.position.x);
-    min.y = std::min(min.y, mass.position.y);
-    max.y = std::max(max.y, mass.position.y);
-    min.z = std::min(min.z, mass.position.z);
-    max.z = std::max(max.z, mass.position.z);
-  }
-
-  // Pad the bounding box
-  float pad = 1.0;
-  min = Vector3Subtract(min, Vector3Scale(Vector3One(), pad));
-  max = Vector3Add(max, Vector3Scale(Vector3One(), pad));
-
-  // Make it cubic (so subdivisions are balanced)
-  float max_extent = std::max({max.x - min.x, max.y - min.y, max.z - min.z});
-  max = Vector3Add(min, Vector3Scale(Vector3One(), max_extent));
-
-  // Root node spans the entire area
-  int root = octree.CreateNode(min, max);
-
-  for (std::size_t i = 0; i < masses.size(); ++i) {
-    octree.Insert(root, i, masses[i].position, MASS);
-  }
-}
-
-auto MassSpringSystem::CalculateRepulsionForces() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  BuildOctree();
-
-  auto solve_octree = [&](int i) {
-    Vector3 force = octree.CalculateForce(0, masses[i].position);
-    masses[i].force = Vector3Add(masses[i].force, force);
-  };
-
-// Calculate forces using Barnes-Hut
-#ifdef THREADPOOL
-  BS::multi_future<void> loop_future =
-      threads.submit_loop(0, masses.size(), solve_octree, 256);
-  loop_future.wait();
-#else
-  for (std::size_t i = 0; i < masses.size(); ++i) {
-    solve_octree(i);
-  }
-#endif
-}
-
-auto MassSpringSystem::VerletUpdate(float delta_time) -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  for (auto &mass : masses) {
-    mass.VerletUpdate(delta_time);
-  }
-}
-
-auto ThreadedPhysics::PhysicsThread(ThreadedPhysics::PhysicsState &state)
-    -> void {
-#ifdef THREADPOOL
-  BS::this_thread::set_os_thread_name("physics");
-#endif
-
-  MassSpringSystem mass_springs;
-
-  const auto visitor = overloads{
-      [&](const struct AddMass &am) { mass_springs.AddMass(); },
-      [&](const struct AddSpring &as) { mass_springs.AddSpring(as.a, as.b); },
-      [&](const struct ClearGraph &cg) { mass_springs.Clear(); },
-  };
-
-  std::chrono::time_point last = std::chrono::high_resolution_clock::now();
-  std::chrono::duration<double> accumulator(0);
-  std::chrono::duration<double> update_accumulator(0);
-  unsigned int updates = 0;
-
-  while (state.running.load()) {
-#ifdef TRACY
-    FrameMarkStart("PhysicsThread");
-#endif
-
-    // Time tracking
-    std::chrono::time_point now = std::chrono::high_resolution_clock::now();
-    std::chrono::duration<double> deltatime = now - last;
-    accumulator += deltatime;
-    update_accumulator += deltatime;
-    last = now;
-
-    // Handle queued commands
-    {
-#ifdef TRACY
-      std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
-#else
-      std::lock_guard<std::mutex> lock(state.command_mtx);
-#endif
-      while (!state.pending_commands.empty()) {
-        Command &cmd = state.pending_commands.front();
-        cmd.visit(visitor);
-        state.pending_commands.pop();
-      }
-    }
-
-    if (mass_springs.masses.empty()) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(1));
-      continue;
-    }
-
-    // Physics update
-    if (accumulator.count() > TIMESTEP) {
-      mass_springs.ClearForces();
-      mass_springs.CalculateSpringForces();
-      mass_springs.CalculateRepulsionForces();
-      mass_springs.VerletUpdate(TIMESTEP * SIM_SPEED);
-
-      ++updates;
-      accumulator -= std::chrono::duration<double>(TIMESTEP);
-    }
-
-    // Publish the positions for the renderer (copy)
-#ifdef TRACY
-    FrameMarkStart("PhysicsThreadProduceLock");
-#endif
-    {
-#ifdef TRACY
-      std::unique_lock<LockableBase(std::mutex)> lock(state.data_mtx);
-#else
-      std::unique_lock<std::mutex> lock(state.data_mtx);
-#endif
-      state.data_consumed_cnd.wait(
-          lock, [&] { return state.data_consumed || !state.running.load(); });
-      if (!state.running.load()) {
-        // Running turned false while we were waiting for the condition
-        break;
-      }
-
-      if (update_accumulator.count() > 1.0) {
-        // Update each second
-        state.ups = updates;
-        updates = 0;
-        update_accumulator = std::chrono::duration<double>(0);
-      }
-
-      state.masses.clear();
-      state.masses.reserve(mass_springs.masses.size());
-      for (const auto &mass : mass_springs.masses) {
-        state.masses.emplace_back(mass.position);
-      }
-
-      state.data_ready = true;
-      state.data_consumed = false;
-    }
-    // Notify the rendering thread that new data is available
-    state.data_ready_cnd.notify_all();
-#ifdef TRACY
-    FrameMarkEnd("PhysicsThreadProduceLock");
-
-    FrameMarkEnd("PhysicsThread");
-#endif
-  }
-}
-
-auto ThreadedPhysics::AddMassCmd() -> void {
-  {
-#ifdef TRACY
-    std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
-#else
-    std::lock_guard<std::mutex> lock(state.command_mtx);
-#endif
-    state.pending_commands.push(AddMass{});
-  }
-}
-
-auto ThreadedPhysics::AddSpringCmd(std::size_t a, std::size_t b) -> void {
-  {
-#ifdef TRACY
-    std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
-#else
-    std::lock_guard<std::mutex> lock(state.command_mtx);
-#endif
-    state.pending_commands.push(AddSpring{a, b});
-  }
-}
-
-auto ThreadedPhysics::ClearCmd() -> void {
-  {
-#ifdef TRACY
-    std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
-#else
-    std::lock_guard<std::mutex> lock(state.command_mtx);
-#endif
-    state.pending_commands.push(ClearGraph{});
-  }
-}
-
-auto ThreadedPhysics::AddMassSpringsCmd(
-    std::size_t num_masses,
-    const std::vector<std::pair<std::size_t, std::size_t>> &springs) -> void {
-  {
-#ifdef TRACY
-    std::lock_guard<LockableBase(std::mutex)> lock(state.command_mtx);
-#else
-    std::lock_guard<std::mutex> lock(state.command_mtx);
-#endif
-    for (std::size_t i = 0; i < num_masses; ++i) {
-      state.pending_commands.push(AddMass{});
-    }
-    for (const auto &[from, to] : springs) {
-      state.pending_commands.push(AddSpring{from, to});
-    }
-  }
-}

src/renderer.cpp

@@ -1,420 +1,343 @@
 #include "renderer.hpp"
 #include "config.hpp"
-#include "puzzle.hpp"
 
-#include <algorithm>
-#include <format>
 #include <raylib.h>
 #include <raymath.h>
 #include <rlgl.h>
+#include <GL/glew.h>
 
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-#ifdef BATCHING
-#include <cstring>
-#endif
-
-auto Renderer::UpdateTextureSizes() -> void {
-  if (!IsWindowResized()) {
-    return;
-  }
-
-  UnloadRenderTexture(render_target);
-  UnloadRenderTexture(klotski_target);
-  UnloadRenderTexture(menu_target);
-
-  int width = GetScreenWidth() / 2.0;
-  int height = GetScreenHeight() - MENU_HEIGHT;
-
-  render_target = LoadRenderTexture(width, height);
-  klotski_target = LoadRenderTexture(width, height);
-  menu_target = LoadRenderTexture(width * 2, MENU_HEIGHT);
-}
-
-auto Renderer::AllocateGraphInstancing(std::size_t size) -> void {
-  cube_instance = GenMeshCube(VERTEX_SIZE, VERTEX_SIZE, VERTEX_SIZE);
-
-  instancing_shader = LoadShader("shader/instancing_vertex.glsl",
-                                 "shader/instancing_fragment.glsl");
-  instancing_shader.locs[SHADER_LOC_MATRIX_MVP] =
-      GetShaderLocation(instancing_shader, "mvp");
-  instancing_shader.locs[SHADER_LOC_VECTOR_VIEW] =
-      GetShaderLocation(instancing_shader, "viewPos");
-
-  vertex_mat = LoadMaterialDefault();
-  vertex_mat.maps[MATERIAL_MAP_DIFFUSE].color = VERTEX_COLOR;
-  vertex_mat.shader = instancing_shader;
-
-  transforms = (Matrix *)MemAlloc(size * sizeof(Matrix));
-  transforms_size = size;
-}
-
-auto Renderer::ReallocateGraphInstancingIfNecessary(std::size_t size) -> void {
-  if (transforms_size != size) {
-    transforms = (Matrix *)MemRealloc(transforms, size * sizeof(Matrix));
-    transforms_size = size;
-  }
-}
-
-auto Renderer::DrawMassSprings(const std::vector<Vector3> &masses) -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  // Prepare cube instancing
-  {
-#ifdef TRACY
-    ZoneNamedN(prepare_masses, "PrepareMasses", true);
-#endif
-    if (masses.size() < DRAW_VERTICES_LIMIT) {
-      if (transforms == nullptr) {
-        AllocateGraphInstancing(masses.size());
-      }
-      ReallocateGraphInstancingIfNecessary(masses.size());
-
-      int i = 0;
-      for (const Vector3 &mass : masses) {
-        transforms[i] = MatrixTranslate(mass.x, mass.y, mass.z);
-        ++i;
-      }
+auto renderer::update_texture_sizes() -> void
+{
+    if (!IsWindowResized()) {
+        return;
     }
-  }
 
-  BeginTextureMode(render_target);
-  ClearBackground(RAYWHITE);
+    UnloadRenderTexture(graph_target);
+    UnloadRenderTexture(klotski_target);
+    UnloadRenderTexture(menu_target);
 
-  BeginMode3D(camera.camera);
+    const int width = GetScreenWidth() / 2;
+    const int height = GetScreenHeight() - MENU_HEIGHT;
 
-  // Draw springs (batched)
-  {
-#ifdef TRACY
-    ZoneNamedN(draw_springs, "DrawSprings", true);
-#endif
+    graph_target = LoadRenderTexture(width, height);
+    klotski_target = LoadRenderTexture(width, height);
+    menu_target = LoadRenderTexture(width * 2, MENU_HEIGHT);
+}
+
+auto renderer::draw_mass_springs(const std::vector<Vector3>& masses) -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    if (masses.size() != state.get_state_count()) {
+        // Because the physics run in a different thread, it might need time to catch up
+        return;
+    }
+
+    // Prepare edge buffer
+    {
+        #ifdef TRACY
+        ZoneNamedN(prepare_edge_buffers, "PrepareEdgeBuffers", true);
+        #endif
+
+        edge_vertices.clear();
+        for (const auto& [from, to] : state.get_links()) {
+            edge_vertices.push_back(masses[from]);
+            edge_vertices.push_back(masses[to]);
+        }
+        rlUpdateVertexBuffer(edge_vbo_id, edge_vertices.data(), edge_vertices.size() * sizeof(Vector3), 0);
+    }
+
+    // Prepare connection drawing
+    {
+        #ifdef TRACY
+        ZoneNamedN(prepare_connections, "PrepareConnectionsDrawing", true);
+        #endif
+
+        connections.clear();
+        connections.reserve(state.get_target_count());
+        if (input.connect_solutions) {
+            for (const size_t& _state : state.get_winning_indices()) {
+                const Vector3& current_mass = masses[state.get_current_index()];
+                const Vector3& winning_mass = masses[_state];
+                connections.emplace_back(current_mass, winning_mass);
+            }
+        }
+    }
+
+    // Prepare cube instancing
+    {
+        #ifdef TRACY
+        ZoneNamedN(prepare_masses, "PrepareMassInstancing", true);
+        #endif
+
+        if (masses.size() < DRAW_VERTICES_LIMIT) {
+            // Don't have to reserve, capacity is already set to DRAW_VERTICES_LIMIT in constructor
+            transforms.clear();
+            colors.clear();
+
+            // Collisions
+            // TODO: This would benefit greatly from a spatial data structure.
+            //       Would it be worth to copy the octree from the physics thread?
+            input.collision_mass = -1;
+            if (input.mouse_in_graph_pane() && IsKeyDown(KEY_Q)) {
+                #ifdef TRACY
+                ZoneNamedN(mass_collisions, "MassCollisions", true);
+                #endif
+                const Ray ray = GetScreenToWorldRayEx(
+                    GetMousePosition() - Vector2(GetScreenWidth() / 2.0f, MENU_HEIGHT),
+                    camera.camera,
+                    graph_target.texture.width,
+                    graph_target.texture.height);
+                // Ray collision hit info
+
+                size_t mass = 0;
+                for (const auto& [x, y, z] : masses) {
+                    const RayCollision collision = GetRayCollisionBox(ray,
+                                                                      BoundingBox{
+                                                                          {
+                                                                              x - VERTEX_SIZE / 2.0f,
+                                                                              y - VERTEX_SIZE / 2.0f,
+                                                                              z - VERTEX_SIZE / 2.0f
+                                                                          },
+                                                                          {
+                                                                              x + VERTEX_SIZE / 2.0f,
+                                                                              y + VERTEX_SIZE / 2.0f,
+                                                                              z + VERTEX_SIZE / 2.0f
+                                                                          }
+                                                                      });
+                    if (collision.hit) {
+                        input.collision_mass = mass;
+                        break;
+                    }
+                    ++mass;
+                }
+            }
+
+            // Find max distance to interpolate colors in the given [0, max] range
+            int max_distance = 0;
+            for (const int distance : state.get_distances()) {
+                if (distance > max_distance) {
+                    max_distance = distance;
+                }
+            }
+
+            const auto lerp_color = [&](const Color from, const Color to, const int distance)
+            {
+                const float weight = 1.0 - static_cast<float>(distance) / max_distance;
+
+                Color result;
+                result.r = static_cast<u8>((1 - weight) * from.r + weight * to.r);
+                result.g = static_cast<u8>((1 - weight) * from.g + weight * to.g);
+                result.b = static_cast<u8>((1 - weight) * from.b + weight * to.b);
+                result.a = static_cast<u8>((1 - weight) * from.a + weight * to.a);
+
+                return result;
+            };
+
+            const std::vector<int>& distances = state.get_distances();
+
+            size_t mass = 0;
+            for (const auto& [x, y, z] : masses) {
+                transforms.emplace_back(MatrixTranslate(x, y, z));
+
+                // Normal vertex
+                Color c = VERTEX_COLOR;
+                if ((input.mark_solutions || input.mark_path) && state.get_winning_indices().contains(mass)) {
+                    // Winning vertex
+                    c = VERTEX_TARGET_COLOR;
+                } else if ((input.mark_solutions || input.mark_path) && state.get_path_indices().contains(mass)) {
+                    // Path vertex
+                    c = VERTEX_PATH_COLOR;
+                } else if (mass == state.get_starting_index()) {
+                    // Starting vertex
+                    c = VERTEX_START_COLOR;
+                } else if (state.get_visit_counts().at(mass) > 0) {
+                    // Visited vertex
+                    c = VERTEX_VISITED_COLOR;
+                } else if (input.color_by_distance && distances.size() == masses.size()) {
+                    c = lerp_color(VERTEX_FARTHEST_COLOR, VERTEX_CLOSEST_COLOR, static_cast<float>(distances[mass]));
+                }
+                if (mass == input.collision_mass) {
+                    c = RED;
+                }
+
+                // Current vertex is drawn as individual cube to increase its size
+                colors.emplace_back(c);
+                ++mass;
+            }
+        }
+
+        rlUpdateVertexBuffer(color_vbo_id, colors.data(), colors.size() * sizeof(Color), 0);
+    }
+
+    BeginTextureMode(graph_target);
+    ClearBackground(RAYWHITE);
+    BeginMode3D(camera.camera);
+
+    rlDrawRenderBatchActive();
+
+    // Draw edges
+    {
+        #ifdef TRACY
+        ZoneNamedN(draw_springs, "DrawSprings", true);
+        #endif
+
+        rlEnableShader(edge_shader.id);
+
+        Matrix modelview = rlGetMatrixModelview();
+        Matrix projection = rlGetMatrixProjection();
+        Matrix mvp = MatrixMultiply(modelview, projection);
+        rlSetUniformMatrix(edge_shader.locs[SHADER_LOC_MATRIX_MVP], mvp);
+
+        const std::array<float, 4> edge_color = {
+            EDGE_COLOR.r / 255.0f,
+            EDGE_COLOR.g / 255.0f,
+            EDGE_COLOR.b / 255.0f,
+            EDGE_COLOR.a / 255.0f
+        };
+        rlSetUniform(edge_color_loc, edge_color.data(), SHADER_UNIFORM_VEC4, 1);
+
+        glBindVertexArray(edge_vao_id);
+        glDrawArrays(GL_LINES, 0, edge_vertices.size());
+        glBindVertexArray(0);
+
+        rlDisableShader();
+
+        // This draws triangles:
+        // rlEnableVertexArray(edge_vao_id);
+        // rlColor4ub(EDGE_COLOR.r, EDGE_COLOR.g, EDGE_COLOR.b, EDGE_COLOR.a);
+        // rlDrawVertexArray(0, edge_vertices.size());
+        // rlDisableVertexArray();
+
+        // This is fucking slow:
+        // rlBegin(RL_LINES);
+        // for (const auto& [from, to] : state.get_links()) {
+        //     if (masses.size() > from && masses.size() > to) {
+        //         const auto& [ax, ay, az] = masses[from];
+        //         const auto& [bx, by, bz] = masses[to];
+        //         rlColor4ub(EDGE_COLOR.r, EDGE_COLOR.g, EDGE_COLOR.b, EDGE_COLOR.a);
+        //         rlVertex3f(ax, ay, az);
+        //         rlVertex3f(bx, by, bz);
+        //     }
+        // }
+        // rlEnd();
+    }
+
+    // Draw masses (instanced)
+    {
+        #ifdef TRACY
+        ZoneNamedN(draw_masses, "DrawMasses", true);
+        #endif
+
+        if (masses.size() < DRAW_VERTICES_LIMIT) {
+            // NOTE: I don't know if drawing all this inside a shader would make it
+            //       much faster... The amount of data sent to the GPU would be
+            //       reduced (just positions instead of matrices), but is this
+            //       noticable for < 100000 cubes?
+            DrawMeshInstanced(cube_instance, vertex_mat, transforms.data(), masses.size()); // NOLINT(*-narrowing-conversions)
+        }
+    }
+
+    // Connect current to winning states (batched)
+    const auto [r, g, b, a] = Fade(VERTEX_CURRENT_COLOR, 0.3);
     rlBegin(RL_LINES);
-    for (const auto &[from, to] : state.springs) {
-      if (masses.size() > from && masses.size() > to) {
-        const Vector3 &a = masses.at(from);
-        const Vector3 &b = masses.at(to);
-        rlColor4ub(EDGE_COLOR.r, EDGE_COLOR.g, EDGE_COLOR.b, EDGE_COLOR.a);
-        rlVertex3f(a.x, a.y, a.z);
-        rlVertex3f(b.x, b.y, b.z);
-      }
+    for (const auto& [from, to] : connections) {
+        const auto& [ax, ay, az] = from;
+        const auto& [bx, by, bz] = to;
+        rlColor4ub(r, g, b, a);
+        rlVertex3f(ax, ay, az);
+        rlVertex3f(bx, by, bz);
     }
     rlEnd();
-  }
 
-  // Draw masses (instanced)
-  {
-#ifdef TRACY
-    ZoneNamedN(draw_masses, "DrawMasses", true);
-#endif
-    if (masses.size() < DRAW_VERTICES_LIMIT) {
-      // NOTE: I don't know if drawing all this inside a shader would make it
-      //       much faster... The amount of data sent to the GPU would be
-      //       reduced (just positions instead of matrices), but is this
-      //       noticable for < 100000 cubes?
-      DrawMeshInstanced(cube_instance, vertex_mat, transforms, masses.size());
+    // Mark current state
+    const size_t current_index = state.get_current_index();
+    if (masses.size() > current_index) {
+        const Vector3& current_mass = masses[current_index];
+        DrawCube(current_mass, VERTEX_SIZE * 2, VERTEX_SIZE * 2, VERTEX_SIZE * 2, VERTEX_CURRENT_COLOR);
     }
-  }
 
-  // Mark winning states
-  if (input.mark_solutions || input.connect_solutions) {
-    for (const State &_state : state.winning_states) {
-
-      std::size_t winning_index = state.states.at(_state);
-      if (masses.size() > winning_index) {
-
-        const Vector3 &winning_mass = masses.at(winning_index);
-        if (input.mark_solutions) {
-          DrawCube(winning_mass, 2 * VERTEX_SIZE, 2 * VERTEX_SIZE,
-                   2 * VERTEX_SIZE, TARGET_BLOCK_COLOR);
-        }
-
-        std::size_t current_index = state.CurrentMassIndex();
-        if (input.connect_solutions && masses.size() > current_index) {
-          const Vector3 &current_mass = masses.at(current_index);
-          DrawLine3D(winning_mass, current_mass, ORANGE);
-        }
-      }
-    }
-  }
-
-  // Mark visited states
-  for (const State &_state : state.visited_states) {
-    std::size_t visited_index = state.states.at(_state);
-
-    if (masses.size() > visited_index) {
-      const Vector3 &visited_mass = masses.at(visited_index);
-      DrawCube(visited_mass, VERTEX_SIZE * 1.5, VERTEX_SIZE * 1.5,
-               VERTEX_SIZE * 1.5, PURPLE);
-    }
-  }
-
-  // Mark winning path
-  if (input.mark_path) {
-    for (const std::size_t &_state : state.winning_path) {
-      if (masses.size() > _state) {
-        const Vector3 &path_mass = masses.at(_state);
-        DrawCube(path_mass, VERTEX_SIZE * 1.75, VERTEX_SIZE * 1.75,
-                 VERTEX_SIZE * 1.75, YELLOW);
-      }
-    }
-  }
-
-  // Mark starting state
-  std::size_t starting_index = state.states.at(state.starting_state);
-  if (masses.size() > starting_index) {
-    const Vector3 &starting_mass = masses.at(starting_index);
-    DrawCube(starting_mass, VERTEX_SIZE * 2, VERTEX_SIZE * 2, VERTEX_SIZE * 2,
-             ORANGE);
-  }
-
-  // Mark current state
-  std::size_t current_index = state.states.at(state.current_state);
-  if (masses.size() > current_index) {
-    const Vector3 &current_mass = masses.at(current_index);
-    DrawCube(current_mass, VERTEX_SIZE * 2, VERTEX_SIZE * 2, VERTEX_SIZE * 2,
-             BLUE);
-  }
-
-  // DrawCubeWires(current_mass.position, REPULSION_RANGE, REPULSION_RANGE,
-  //               REPULSION_RANGE, BLACK);
-  // DrawGrid(100, 1.0);
-  // DrawSphere(camera.target, VERTEX_SIZE, ORANGE);
-  EndMode3D();
-
-  DrawLine(0, 0, 0, GetScreenHeight() - MENU_HEIGHT, BLACK);
-  EndTextureMode();
+    EndMode3D();
+    EndTextureMode();
 }
 
-auto Renderer::DrawKlotski() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
+auto renderer::draw_klotski() const -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
 
-  BeginTextureMode(klotski_target);
-  ClearBackground(RAYWHITE);
+    BeginTextureMode(klotski_target);
+    ClearBackground(RAYWHITE);
 
-  // Draw Board
-  const int board_width = GetScreenWidth() / 2 - 2 * BOARD_PADDING;
-  const int board_height = GetScreenHeight() - MENU_HEIGHT - 2 * BOARD_PADDING;
-  int block_size = std::min(board_width / state.current_state.width,
-                            board_height / state.current_state.height) -
-                   2 * BLOCK_PADDING;
-  int x_offset = (board_width - (block_size + 2 * BLOCK_PADDING) *
-                                    state.current_state.width) /
-                 2.0;
-  int y_offset = (board_height - (block_size + 2 * BLOCK_PADDING) *
-                                     state.current_state.height) /
-                 2.0;
+    gui.draw_puzzle_board();
 
-  DrawRectangle(0, 0, GetScreenWidth() / 2, GetScreenHeight() - MENU_HEIGHT,
-                RAYWHITE);
-  DrawRectangle(x_offset, y_offset,
-                board_width - 2 * x_offset + 2 * BOARD_PADDING,
-                board_height - 2 * y_offset + 2 * BOARD_PADDING,
-                state.current_state.IsWon()
-                    ? GREEN
-                    : (state.current_state.restricted ? DARKGRAY : LIGHTGRAY));
-  for (int x = 0; x < state.current_state.width; ++x) {
-    for (int y = 0; y < state.current_state.height; ++y) {
-      DrawRectangle(x_offset + BOARD_PADDING + x * BLOCK_PADDING * 2 +
-                        BLOCK_PADDING + x * block_size,
-                    y_offset + BOARD_PADDING + y * BLOCK_PADDING * 2 +
-                        BLOCK_PADDING + y * block_size,
-                    block_size, block_size, WHITE);
-    }
-  }
-
-  // Draw Blocks
-  for (Block block : state.current_state) {
-    Color c = BLOCK_COLOR;
-    if (block.Covers(input.sel_x, input.sel_y)) {
-      c = HL_BLOCK_COLOR;
-    }
-    if (block.target) {
-      if (block.Covers(input.sel_x, input.sel_y)) {
-        c = HL_TARGET_BLOCK_COLOR;
-      } else {
-        c = TARGET_BLOCK_COLOR;
-      }
-    } else if (block.immovable) {
-      if (block.Covers(input.sel_x, input.sel_y)) {
-        c = HL_WALL_COLOR;
-      } else {
-        c = WALL_COLOR;
-      }
-    }
-    DrawRectangle(x_offset + BOARD_PADDING + block.x * BLOCK_PADDING * 2 +
-                      BLOCK_PADDING + block.x * block_size,
-                  y_offset + BOARD_PADDING + block.y * BLOCK_PADDING * 2 +
-                      BLOCK_PADDING + block.y * block_size,
-                  block.width * block_size + block.width * 2 * BLOCK_PADDING -
-                      2 * BLOCK_PADDING,
-                  block.height * block_size + block.height * 2 * BLOCK_PADDING -
-                      2 * BLOCK_PADDING,
-                  c);
-
-    if (block.Covers(input.hov_x, input.hov_y)) {
-      DrawRectangleLinesEx(
-          Rectangle(x_offset + BOARD_PADDING + block.x * BLOCK_PADDING * 2 +
-                        BLOCK_PADDING + block.x * block_size,
-                    y_offset + BOARD_PADDING + block.y * BLOCK_PADDING * 2 +
-                        BLOCK_PADDING + block.y * block_size,
-                    block.width * block_size + block.width * 2 * BLOCK_PADDING -
-                        2 * BLOCK_PADDING,
-                    block.height * block_size +
-                        block.height * 2 * BLOCK_PADDING - 2 * BLOCK_PADDING),
-          2.0, BLACK);
-    }
-  }
-
-  // Draw editing starting position
-  if (input.block_add_x >= 0 && input.block_add_y >= 0 &&
-      input.hov_x >= input.block_add_x && input.hov_y >= input.block_add_y) {
-    int block_width = input.hov_x - input.block_add_x + 1;
-    int block_height = input.hov_y - input.block_add_y + 1;
-    DrawRectangle(
-        x_offset + BOARD_PADDING + input.block_add_x * BLOCK_PADDING * 2 +
-            BLOCK_PADDING + input.block_add_x * block_size,
-        y_offset + BOARD_PADDING + input.block_add_y * BLOCK_PADDING * 2 +
-            BLOCK_PADDING + input.block_add_y * block_size,
-        block_width * block_size + block_width * 2 * BLOCK_PADDING -
-            2 * BLOCK_PADDING,
-        block_height * block_size + block_height * 2 * BLOCK_PADDING -
-            2 * BLOCK_PADDING,
-        Fade(BLOCK_COLOR, 0.5));
-  }
-
-  // Draw board goal position
-  const Block target = state.current_state.GetTargetBlock();
-  if (target.IsValid() && state.current_state.HasWinCondition()) {
-    int target_x = state.current_state.target_x;
-    int target_y = state.current_state.target_y;
-    DrawRectangleLinesEx(
-        Rectangle(x_offset + BOARD_PADDING + target_x * BLOCK_PADDING * 2 +
-                      BLOCK_PADDING + target_x * block_size,
-                  y_offset + BOARD_PADDING + target_y * BLOCK_PADDING * 2 +
-                      BLOCK_PADDING + target_y * block_size,
-                  target.width * block_size + target.width * 2 * BLOCK_PADDING -
-                      2 * BLOCK_PADDING,
-                  target.height * block_size +
-                      target.height * 2 * BLOCK_PADDING - 2 * BLOCK_PADDING),
-        2.0, TARGET_BLOCK_COLOR);
-  }
-
-  DrawLine(GetScreenWidth() / 2 - 1, 0, GetScreenWidth() / 2 - 1,
-           GetScreenHeight() - MENU_HEIGHT, BLACK);
-  EndTextureMode();
+    EndTextureMode();
 }
 
-auto Renderer::DrawMenu(const std::vector<Vector3> &masses) -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
+auto renderer::draw_menu() const -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
 
-  BeginTextureMode(menu_target);
-  ClearBackground(RAYWHITE);
+    BeginTextureMode(menu_target);
+    ClearBackground(RAYWHITE);
 
-  float btn_width =
-      static_cast<float>(GetScreenWidth() - (MENU_COLS * MENU_PAD + MENU_PAD)) /
-      MENU_COLS;
-  float btn_height =
-      static_cast<float>(MENU_HEIGHT - (MENU_ROWS * MENU_PAD + MENU_PAD)) /
-      MENU_ROWS;
+    gui.draw_main_menu();
 
-  auto draw_btn = [&](int x, int y, std::string text, Color color) {
-    int posx = MENU_PAD + x * (MENU_PAD + btn_width);
-    int posy = MENU_PAD + (y + 1) * (MENU_PAD + btn_height);
-    DrawRectangle(posx, posy, btn_width, btn_height, Fade(color, 0.7));
-    DrawRectangleLines(posx, posy, btn_width, btn_height, color);
-    DrawText(text.data(), posx + BUTTON_PAD, posy + BUTTON_PAD,
-             btn_height - 2.0 * BUTTON_PAD, WHITE);
-  };
-
-  auto draw_subtitle = [&](std::string text, Color color) {
-    int posx = MENU_PAD;
-    int posy = MENU_PAD;
-    DrawRectangle(posx, posy,
-                  btn_width * MENU_COLS + MENU_PAD * (MENU_COLS - 1),
-                  btn_height, Fade(color, 0.7));
-    DrawRectangleLines(posx, posy,
-                       btn_width * MENU_COLS + MENU_PAD * (MENU_COLS - 1),
-                       btn_height, color);
-    DrawText(text.data(), posx + BUTTON_PAD, posy + BUTTON_PAD,
-             btn_height = 2.0 * BUTTON_PAD, WHITE);
-  };
-
-  // Left column
-  draw_btn(0, 0,
-           std::format("States: {} / Transitions: {} / Winning: {}",
-                       masses.size(), state.springs.size(),
-                       state.winning_states.size()),
-           ORANGE);
-  draw_btn(0, 1,
-           std::format("Preset (M/N) / {} (F)",
-                       state.current_state.restricted ? "Restricted" : "Free"),
-           ORANGE);
-  draw_btn(0, 2, std::format("Pan (LMB) / Rotate (RMB) / Zoom (Wheel)"),
-           DARKGREEN);
-  draw_btn(
-      0, 3,
-      std::format("Lock Camera to Current State (L): {}", camera.target_lock),
-      DARKGREEN);
-
-  // Center column
-  draw_btn(1, 0, std::format("Select (LMB) / Move (WASD) / Target/Wall (T/Y)"),
-           DARKBLUE);
-  draw_btn(1, 1, std::format("Add/Remove Col/Row (Arrow Keys)"), DARKBLUE);
-  draw_btn(1, 2, std::format("Add/Remove Block (LMB/RMB) / Set Goal (MMB)"),
-           DARKBLUE);
-  draw_btn(1, 3, std::format("Print State (P) / Reset State (R)"), DARKBLUE);
-
-  // Right column
-  draw_btn(2, 0, std::format("Populate Graph (G) / Clear Graph (C)"),
-           DARKPURPLE);
-  draw_btn(2, 1,
-           std::format("Path (U): {} / Goals (I): {} / Lines (O): {}",
-                       input.mark_path, input.mark_solutions,
-                       input.connect_solutions),
-           DARKPURPLE);
-  draw_btn(2, 2, std::format("Best move (Space) / Move back (Backspace)"),
-           DARKPURPLE);
-  draw_btn(2, 3,
-           std::format("Worst (V) / Target (B) / Distance: {}",
-                       state.winning_path.size() > 0
-                           ? state.winning_path.size() - 1
-                           : 0),
-           DARKPURPLE);
-
-  draw_subtitle(std::format("Puzzle {}: {}", state.current_preset + 1,
-                            state.comments.at(state.current_preset)),
-                BLACK);
-
-  DrawLine(0, MENU_HEIGHT - 1, GetScreenWidth(), MENU_HEIGHT - 1, BLACK);
-  EndTextureMode();
+    EndTextureMode();
 }
 
-auto Renderer::DrawTextures(float ups) -> void {
-  BeginDrawing();
-  DrawTextureRec(menu_target.texture,
-                 Rectangle(0, 0, menu_target.texture.width,
-                           -1 * menu_target.texture.height),
-                 Vector2(0, 0), WHITE);
-  DrawTextureRec(klotski_target.texture,
-                 Rectangle(0, 0, klotski_target.texture.width,
-                           -1 * klotski_target.texture.height),
-                 Vector2(0, MENU_HEIGHT), WHITE);
-  DrawTextureRec(render_target.texture,
-                 Rectangle(0, 0, render_target.texture.width,
-                           -1 * render_target.texture.height),
-                 Vector2(GetScreenWidth() / 2.0, MENU_HEIGHT), WHITE);
+auto renderer::draw_textures(const int fps,
+                             const int ups,
+                             const size_t mass_count,
+                             const size_t spring_count) const -> void
+{
+    BeginDrawing();
 
-  DrawFPS(GetScreenWidth() / 2 + 10, MENU_HEIGHT + 10);
-  DrawText(TextFormat("%.0f UPS", ups), GetScreenWidth() / 2 + 120,
-           MENU_HEIGHT + 10, 20, ORANGE);
-  EndDrawing();
+    DrawTextureRec(menu_target.texture,
+                   Rectangle(0, 0, menu_target.texture.width, -menu_target.texture.height),
+                   Vector2(0, 0),
+                   WHITE);
+    DrawTextureRec(klotski_target.texture,
+                   Rectangle(0, 0, klotski_target.texture.width, -klotski_target.texture.height),
+                   Vector2(0, MENU_HEIGHT),
+                   WHITE);
+    DrawTextureRec(graph_target.texture,
+                   Rectangle(0, 0, graph_target.texture.width, -graph_target.texture.height),
+                   Vector2(GetScreenWidth() / 2.0f, MENU_HEIGHT),
+                   WHITE);
+
+    // Draw borders
+    DrawRectangleLinesEx(Rectangle(0, 0, GetScreenWidth(), MENU_HEIGHT), 1.0f, BLACK);
+    DrawRectangleLinesEx(Rectangle(0, MENU_HEIGHT, GetScreenWidth() / 2.0f, GetScreenHeight() - MENU_HEIGHT),
+                         1.0f,
+                         BLACK);
+    DrawRectangleLinesEx(Rectangle(GetScreenWidth() / 2.0f,
+                                   MENU_HEIGHT,
+                                   GetScreenWidth() / 2.0f,
+                                   GetScreenHeight() - MENU_HEIGHT),
+                         1.0f,
+                         BLACK);
+
+    gui.draw(fps, ups, mass_count, spring_count);
+
+    EndDrawing();
 }
+
+auto renderer::render(const std::vector<Vector3>& masses,
+                      const int fps,
+                      const int ups,
+                      const size_t mass_count,
+                      const size_t spring_count) -> void
+{
+    update_texture_sizes();
+
+    draw_mass_springs(masses);
+    draw_klotski();
+    draw_menu();
+    draw_textures(fps, ups, mass_count, spring_count);
+}

src/state.cpp

@@ -1,258 +0,0 @@
-#include "state.hpp"
-#include "config.hpp"
-#include "distance.hpp"
-
-#include <fstream>
-#include <ios>
-#include <raymath.h>
-
-#ifdef TRACY
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-#endif
-
-auto StateManager::ParsePresetFile(const std::string &preset_file) -> void {
-  std::ifstream file(preset_file);
-  if (!file) {
-    std::cout << "Preset file \"" << preset_file << "\" couldn't be loaded."
-              << std::endl;
-    return;
-  }
-
-  std::string line;
-  std::vector<std::string> comment_lines;
-  std::vector<std::string> preset_lines;
-  while (std::getline(file, line)) {
-    if (line.starts_with("F") || line.starts_with("R")) {
-      preset_lines.push_back(line);
-    } else if (line.starts_with("#")) {
-      comment_lines.push_back(line);
-    }
-  }
-
-  if (preset_lines.size() == 0 || comment_lines.size() != preset_lines.size()) {
-    std::cout << "Preset file \"" << preset_file << "\" couldn't be loaded."
-              << std::endl;
-    return;
-  }
-
-  presets.clear();
-  for (const auto &preset : preset_lines) {
-    presets.emplace_back(preset);
-  }
-  comments = comment_lines;
-
-  std::cout << "Loaded " << preset_lines.size() << " presets." << std::endl;
-}
-
-auto StateManager::LoadPreset(int preset) -> void {
-  current_preset = preset;
-  current_state = presets.at(current_preset);
-  ClearGraph();
-  edited = false;
-}
-
-auto StateManager::ResetState() -> void {
-  current_state = presets.at(current_preset);
-  previous_state = current_state;
-  if (edited) {
-    // We also need to clear the graph in case the state has been edited
-    // because the graph could contain states that are impossible to reach
-    // now.
-    ClearGraph();
-    edited = false;
-  }
-}
-
-auto StateManager::PreviousPreset() -> void {
-  LoadPreset((presets.size() + current_preset - 1) % presets.size());
-}
-
-auto StateManager::NextPreset() -> void {
-  LoadPreset((current_preset + 1) % presets.size());
-}
-
-auto StateManager::NextPath() -> void {
-  if (target_distances.Empty()) {
-    return;
-  }
-
-  // Already there
-  if (target_distances.distances[CurrentMassIndex()] == 0) {
-    return;
-  }
-
-  std::size_t parent = target_distances.parents[CurrentMassIndex()];
-  // std::cout << "Parent of node " << CurrentMassIndex() << " is " << parent
-  //           << std::endl;
-  current_state = masses.at(parent);
-  FindTargetPath();
-}
-
-auto StateManager::FillGraph() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  ClearGraph();
-
-  std::pair<std::vector<State>,
-            std::vector<std::pair<std::size_t, std::size_t>>>
-      closure = current_state.Closure();
-
-  physics.ClearCmd();
-  physics.AddMassSpringsCmd(closure.first.size(), closure.second);
-  for (const State &state : closure.first) {
-    states.insert(std::make_pair(state, states.size()));
-    masses.insert(std::make_pair(states.size() - 1, state));
-  }
-  for (const auto &[from, to] : closure.second) {
-    springs.emplace_back(from, to);
-  }
-  FindWinningStates();
-  FindTargetDistances();
-  FindTargetPath();
-
-  // Sanity check. Both values need to be equal
-  // for (const auto &[mass, state] : masses) {
-  //   std::cout << "Masses: " << mass << ", States: " << states.at(state)
-  //             << std::endl;
-  // }
-}
-
-auto StateManager::UpdateGraph() -> void {
-  if (previous_state == current_state) {
-    return;
-  }
-
-  if (!states.contains(current_state)) {
-    states.insert(std::make_pair(current_state, states.size()));
-    masses.insert(std::make_pair(states.size() - 1, current_state));
-    springs.emplace_back(states.at(current_state), states.at(previous_state));
-    physics.AddMassCmd();
-    physics.AddSpringCmd(states.at(current_state), states.at(previous_state));
-
-    if (current_state.IsWon()) {
-      winning_states.insert(current_state);
-    }
-    FindTargetDistances();
-  }
-
-  visited_states.insert(current_state);
-
-  if (history.size() > 0 && history.top() == current_state) {
-    // We don't pop the stack when moving backwards to indicate if we need to
-    // push or pop here
-    history.pop();
-  } else {
-    history.push(previous_state);
-  }
-
-  FindTargetPath();
-  previous_state = current_state;
-}
-
-auto StateManager::ClearGraph() -> void {
-  states.clear();
-  winning_states.clear();
-  visited_states.clear();
-  masses.clear();
-  winning_path.clear();
-  springs.clear();
-  history = std::stack<State>();
-  target_distances.Clear();
-  physics.ClearCmd();
-
-  // Re-add the default stuff to the graph
-  states.insert(std::make_pair(current_state, states.size()));
-  masses.insert(std::make_pair(states.size() - 1, current_state));
-  visited_states.insert(current_state);
-  physics.AddMassCmd();
-
-  // These states are no longer in the graph
-  previous_state = current_state;
-  starting_state = current_state;
-}
-
-auto StateManager::FindWinningStates() -> void {
-  winning_states.clear();
-  for (const auto &[state, mass] : states) {
-    if (state.IsWon()) {
-      winning_states.insert(state);
-    }
-  }
-}
-
-auto StateManager::FindTargetDistances() -> void {
-#ifdef TRACY
-  ZoneScoped;
-#endif
-
-  if (springs.size() == 0 || winning_states.size() == 0) {
-    return;
-  }
-
-  // Find target indices
-  std::vector<std::size_t> targets;
-  targets.reserve(winning_states.size());
-  for (const auto &_state : winning_states) {
-    targets.push_back(states.at(_state));
-  }
-
-  target_distances = CalculateDistances(states.size(), springs, targets);
-
-  // std::cout << "Calculated " << target_distances.distances.size()
-  //           << " distances to " << targets.size() << " targets." <<
-  //           std::endl;
-}
-
-auto StateManager::FindTargetPath() -> void {
-  if (target_distances.Empty()) {
-    return;
-  }
-
-  winning_path = GetPath(target_distances, CurrentMassIndex());
-  // std::cout << "Nearest target is " << winning_path.size() << " moves away."
-  //           << std::endl;
-}
-
-auto StateManager::FindWorstState() -> State {
-  if (target_distances.Empty()) {
-    return current_state;
-  }
-
-  int max = 0;
-  int index = 0;
-  for (std::size_t i = 0; i < target_distances.distances.size(); ++i) {
-    if (target_distances.distances.at(i) > max) {
-      max = target_distances.distances.at(i);
-      index = i;
-    }
-  }
-
-  return masses.at(index);
-}
-
-auto StateManager::GoToWorst() -> void { current_state = FindWorstState(); }
-
-auto StateManager::GoToNearestTarget() -> void {
-  if (target_distances.Empty()) {
-    return;
-  }
-
-  current_state =
-      masses.at(target_distances.nearest_targets.at(CurrentMassIndex()));
-}
-
-auto StateManager::PopHistory() -> void {
-  if (history.size() == 0) {
-    return;
-  }
-
-  current_state = history.top();
-  // history.pop(); // Done in UpdateGraph();
-}
-
-auto StateManager::CurrentMassIndex() const -> std::size_t {
-  return states.at(current_state);
-}

src/state_manager.cpp

@@ -0,0 +1,445 @@
+#include "state_manager.hpp"
+#include "graph_distances.hpp"
+#include "util.hpp"
+
+auto state_manager::synced_try_insert_state(const puzzle& state) -> size_t
+{
+    if (state_indices.contains(state)) {
+        return state_indices[state];
+    }
+
+    const size_t index = state_pool.size();
+    state_pool.emplace_back(state);
+    state_indices.emplace(state, index);
+    visit_counts[index] = 0;
+
+    // Queue an update to the physics engine state to keep in sync
+    physics.add_mass_cmd();
+
+    return index;
+}
+
+auto state_manager::synced_insert_link(size_t first_index, size_t second_index) -> void
+{
+    links.emplace_back(first_index, second_index);
+
+    // Queue an update to the physics engine state to keep in sync
+    physics.add_spring_cmd(first_index, second_index);
+}
+
+auto state_manager::synced_insert_statespace(const std::vector<puzzle>& states,
+                                             const std::vector<spring>& _links) -> void
+{
+    if (!state_pool.empty() || !state_indices.empty() || !links.empty()) {
+        warnln("Inserting statespace but collections haven't been cleared");
+    }
+
+    for (const puzzle& state : states) {
+        const size_t index = state_pool.size();
+        state_pool.emplace_back(state);
+        state_indices.emplace(state, index);
+        visit_counts[index] = 0;
+    }
+    for (const auto& [from, to] : _links) {
+        links.emplace_back(from, to);
+    }
+
+    // Queue an update to the physics engine state to keep in sync
+    physics.add_mass_springs_cmd(state_pool.size(), links);
+}
+
+auto state_manager::synced_clear_statespace() -> void
+{
+    // Those are invalid without any states
+    current_state_index = -1;
+    previous_state_index = -1;
+    starting_state_index = -1;
+
+    state_pool.clear();
+    state_indices.clear();
+    links.clear();
+    node_target_distances.clear();
+    winning_indices.clear();
+    winning_path.clear();
+    path_indices.clear();
+
+    move_history.clear();
+    visit_counts.clear();
+
+    // Queue an update to the physics engine state to keep in sync
+    physics.clear_cmd();
+}
+
+auto state_manager::save_current_to_preset_file(const std::string& preset_comment) -> void
+{
+    if (append_preset_file(preset_file, preset_comment, get_current_state())) {
+        current_preset = preset_states.size();
+        reload_preset_file();
+    }
+}
+
+auto state_manager::reload_preset_file() -> void
+{
+    const auto [presets, comments] = parse_preset_file(preset_file);
+    if (!presets.empty()) {
+        preset_states = presets;
+        preset_comments = comments;
+    }
+    load_preset(current_preset);
+}
+
+auto state_manager::load_preset(const size_t preset) -> void
+{
+    clear_graph_and_add_current(preset_states[preset]);
+    current_preset = preset;
+    edited = false;
+}
+
+auto state_manager::load_previous_preset() -> void
+{
+    load_preset((preset_states.size() + current_preset - 1) % preset_states.size());
+}
+
+auto state_manager::load_next_preset() -> void
+{
+    load_preset((current_preset + 1) % preset_states.size());
+}
+
+auto state_manager::update_current_state(const puzzle& p) -> void
+{
+    if (!p.valid()) {
+        return;
+    }
+
+    const size_t size_before = state_pool.size();
+
+    // If state is a duplicate, index will be the existing index,
+    // if state is new, index will be state_pool.size() - 1
+    const size_t index = synced_try_insert_state(p);
+
+    // Because synced_insert_link does not check for duplicates we do it here,
+    // if the size grows, it was not a duplicate, and we can add the spring
+    if (state_pool.size() > size_before) {
+        // The order is important, as the position of the second mass will be updated depending on
+        // the first
+        synced_insert_link(current_state_index, index);
+    }
+
+    previous_state_index = current_state_index;
+    current_state_index = index;
+
+    if (current_state_index != previous_state_index) {
+        move_history.emplace_back(previous_state_index);
+    }
+
+    if (p.goal_reached()) {
+        winning_indices.insert(current_state_index);
+    }
+
+    // Adds the element with 0 if it doesn't exist
+    visit_counts[current_state_index]++;
+    total_moves++;
+
+    // Recalculate distances only if the graph changed
+    if (state_pool.size() > size_before) {
+        populate_node_target_distances();
+    }
+    populate_winning_path();
+}
+
+auto state_manager::edit_starting_state(const puzzle& p) -> void
+{
+    clear_graph_and_add_current(p);
+
+    move_history.clear();
+    total_moves = 0;
+    for (int& visits : visit_counts | std::views::values) {
+        visits = 0;
+    }
+    visit_counts[current_state_index]++;
+
+    edited = true;
+}
+
+auto state_manager::goto_starting_state() -> void
+{
+    update_current_state(get_state(starting_state_index));
+
+    // Reset previous movement data since we're starting over (because we're fucking stupid)
+    previous_state_index = current_state_index;
+    for (int& visits : visit_counts | std::views::values) {
+        visits = 0;
+    }
+    visit_counts[current_state_index]++;
+    move_history.clear();
+    total_moves = 0;
+}
+
+auto state_manager::goto_optimal_next_state() -> void
+{
+    if (node_target_distances.empty()) {
+        return;
+    }
+
+    // Already there
+    if (node_target_distances.distances[current_state_index] == 0) {
+        return;
+    }
+
+    const size_t parent_index = node_target_distances.parents[current_state_index];
+    update_current_state(get_state(parent_index));
+}
+
+auto state_manager::goto_previous_state() -> void
+{
+    if (move_history.empty()) {
+        return;
+    }
+
+    update_current_state(get_state(move_history.back()));
+
+    // Pop twice because update_current_state adds the state again...
+    move_history.pop_back();
+    move_history.pop_back();
+}
+
+auto state_manager::goto_most_distant_state() -> void
+{
+    if (node_target_distances.empty()) {
+        return;
+    }
+
+    int max_distance = 0;
+    size_t max_distance_index = 0;
+    for (size_t i = 0; i < node_target_distances.distances.size(); ++i) {
+        if (node_target_distances.distances[i] > max_distance) {
+            max_distance = node_target_distances.distances[i];
+            max_distance_index = i;
+        }
+    }
+
+    update_current_state(get_state(max_distance_index));
+}
+
+auto state_manager::goto_closest_target_state() -> void
+{
+    if (node_target_distances.empty()) {
+        return;
+    }
+
+    update_current_state(get_state(node_target_distances.nearest_targets[current_state_index]));
+}
+
+auto state_manager::populate_graph() -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    // Need to make a copy before clearing the state_pool
+    const puzzle s = get_starting_state();
+    const puzzle p = get_current_state();
+
+    // Clear the graph first so we don't add duplicates somehow
+    synced_clear_statespace();
+
+    // Explore the entire statespace starting from the current state
+    const std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
+
+    const auto& [states, _links] = s.explore_state_space();
+    synced_insert_statespace(states, _links);
+
+    const std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
+
+    infoln("Explored puzzle. Took {}ms.", std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count());
+    infoln("State space has size {} with {} transitions.", state_pool.size(), links.size());
+
+    current_state_index = state_indices[p];
+    previous_state_index = current_state_index;
+    starting_state_index = state_indices[s];
+
+    // Search for cool stuff
+    populate_winning_indices();
+    populate_node_target_distances();
+    populate_winning_path();
+}
+
+auto state_manager::clear_graph_and_add_current(const puzzle& p) -> void
+{
+    // We need to make a copy before clearing the state_pool
+    const puzzle _p = p; // NOLINT(*-unnecessary-copy-initialization)
+
+    synced_clear_statespace();
+
+    // Re-add the current state
+    current_state_index = synced_try_insert_state(_p);
+
+    // These states are no longer in the graph
+    previous_state_index = current_state_index;
+    starting_state_index = current_state_index;
+
+    visit_counts[current_state_index]++;
+}
+
+auto state_manager::clear_graph_and_add_current() -> void
+{
+    clear_graph_and_add_current(get_current_state());
+}
+
+auto state_manager::populate_winning_indices() -> void
+{
+    winning_indices.clear();
+    for (const auto& [state, index] : state_indices) {
+        if (state.goal_reached()) {
+            winning_indices.insert(index);
+        }
+    }
+}
+
+auto state_manager::populate_node_target_distances() -> void
+{
+    #ifdef TRACY
+    ZoneScoped;
+    #endif
+
+    if (links.empty() || winning_indices.empty()) {
+        return;
+    }
+
+    const std::vector<size_t> targets(winning_indices.begin(), winning_indices.end());
+    node_target_distances.calculate_distances(state_pool.size(), links, targets);
+}
+
+auto state_manager::populate_winning_path() -> void
+{
+    if (node_target_distances.empty()) {
+        return;
+    }
+
+    winning_path = node_target_distances.get_shortest_path(current_state_index);
+
+    path_indices.clear();
+    for (const size_t index : winning_path) {
+        path_indices.insert(index);
+    }
+}
+
+auto state_manager::get_index(const puzzle& state) const -> size_t
+{
+    return state_indices.at(state);
+}
+
+auto state_manager::get_current_index() const -> size_t
+{
+    return current_state_index;
+}
+
+auto state_manager::get_starting_index() const -> size_t
+{
+    return starting_state_index;
+}
+
+auto state_manager::get_state(const size_t index) const -> const puzzle&
+{
+    return state_pool[index];
+}
+
+auto state_manager::get_current_state() const -> const puzzle&
+{
+    return get_state(current_state_index);
+}
+
+auto state_manager::get_starting_state() const -> const puzzle&
+{
+    return get_state(starting_state_index);
+}
+
+auto state_manager::get_state_count() const -> size_t
+{
+    return state_pool.size();
+}
+
+auto state_manager::get_target_count() const -> size_t
+{
+    return winning_indices.size();
+}
+
+auto state_manager::get_link_count() const -> size_t
+{
+    return links.size();
+}
+
+auto state_manager::get_path_length() const -> size_t
+{
+    return winning_path.size();
+}
+
+auto state_manager::get_links() const -> const std::vector<spring>&
+{
+    return links;
+}
+
+auto state_manager::get_winning_indices() const -> const boost::unordered_flat_set<size_t>&
+{
+    return winning_indices;
+}
+
+auto state_manager::get_visit_counts() const -> const boost::unordered_flat_map<size_t, int>&
+{
+    return visit_counts;
+}
+
+auto state_manager::get_winning_path() const -> const std::vector<size_t>&
+{
+    return winning_path;
+}
+
+auto state_manager::get_path_indices() const -> const boost::unordered_flat_set<size_t>&
+{
+    return path_indices;
+}
+
+auto state_manager::get_current_visits() const -> int
+{
+    return visit_counts.at(current_state_index);
+}
+
+auto state_manager::get_current_preset() const -> size_t
+{
+    return current_preset;
+}
+
+auto state_manager::get_preset_count() const -> size_t
+{
+    return preset_states.size();
+}
+
+auto state_manager::get_current_preset_comment() const -> const std::string&
+{
+    return preset_comments[current_preset];
+}
+
+auto state_manager::has_history() const -> bool
+{
+    return !move_history.empty();
+}
+
+auto state_manager::has_distances() const -> bool
+{
+    return !node_target_distances.empty();
+}
+
+auto state_manager::get_distances() const -> std::vector<int>
+{
+    return node_target_distances.distances;
+}
+
+auto state_manager::get_total_moves() const -> size_t
+{
+    return total_moves;
+}
+
+auto state_manager::was_edited() const -> bool
+{
+    return edited;
+}

src/tracy.cpp

@@ -1,22 +0,0 @@
-#include "config.hpp"
-
-#ifdef TRACY
-
-#include "tracy.hpp"
-#include <tracy/Tracy.hpp>
-
-void *operator new(std::size_t count) {
-  auto ptr = malloc(count);
-  TracyAllocS(ptr, count, 20);
-  return ptr;
-}
-void operator delete(void *ptr) noexcept {
-  TracyFreeS(ptr, 20);
-  free(ptr);
-}
-void operator delete(void *ptr, std::size_t count) noexcept {
-  TracyFreeS(ptr, 20);
-  free(ptr);
-}
-
-#endif

src/user_interface.cpp

@@ -0,0 +1,991 @@
+#include "user_interface.hpp"
+#include "config.hpp"
+#include "input_handler.hpp"
+
+#include <raylib.h>
+
+#define RAYGUI_IMPLEMENTATION
+#include <raygui.h>
+
+auto user_interface::grid::update_bounds(const int _x,
+                                         const int _y,
+                                         const int _width,
+                                         const int _height,
+                                         const int _columns,
+                                         const int _rows) -> void
+{
+    x = _x;
+    y = _y;
+    width = _width;
+    height = _height;
+    columns = _columns;
+    rows = _rows;
+}
+
+auto user_interface::grid::update_bounds(const int _x, const int _y, const int _width, const int _height) -> void
+{
+    x = _x;
+    y = _y;
+    width = _width;
+    height = _height;
+}
+
+auto user_interface::grid::update_bounds(const int _x, const int _y) -> void
+{
+    x = _x;
+    y = _y;
+}
+
+auto user_interface::grid::bounds() const -> Rectangle
+{
+    Rectangle bounds{0, 0, static_cast<float>(columns), static_cast<float>(rows)};
+    bounds.x -= padding;
+    bounds.y -= padding;
+    bounds.width += 2 * padding;
+    bounds.height += 2 * padding;
+    return bounds;
+}
+
+auto user_interface::grid::bounds(const int _x, const int _y, const int _width, const int _height) const -> Rectangle
+{
+    if (_x < 0 || _x + _width > columns || _y < 0 || _y + _height > rows) {
+        throw std::invalid_argument("Grid bounds out of range");
+    }
+
+    const int cell_width = (width - padding) / columns;
+    const int cell_height = (height - padding) / rows;
+
+    return Rectangle(x + _x * cell_width + padding,
+                     y + _y * cell_height + padding,
+                     _width * cell_width - padding,
+                     _height * cell_height - padding);
+}
+
+auto user_interface::grid::square_bounds() const -> Rectangle
+{
+    Rectangle bounds = square_bounds(0, 0, columns, rows);
+    bounds.x -= padding;
+    bounds.y -= padding;
+    bounds.width += 2 * padding;
+    bounds.height += 2 * padding;
+    return bounds;
+}
+
+auto user_interface::grid::square_bounds(const int _x,
+                                         const int _y,
+                                         const int _width,
+                                         const int _height) const -> Rectangle
+{
+    // Assumes each cell is square, so either width or height are not completely
+    // filled
+
+    if (_x < 0 || _x + _width > columns || _y < 0 || _y + _height > rows) {
+        throw std::invalid_argument("Grid bounds out of range");
+    }
+
+    const int available_width = width - padding * (columns + 1);
+    const int available_height = height - padding * (rows + 1);
+    const int cell_size = std::min(available_width / columns, available_height / rows);
+
+    const int grid_width = cell_size * columns + padding * (columns + 1);
+    const int grid_height = cell_size * rows + padding * (rows + 1);
+    const int x_offset = (width - grid_width) / 2;
+    const int y_offset = (height - grid_height) / 2;
+
+    return Rectangle(x_offset + _x * (cell_size + padding) + padding,
+                     y_offset + _y * (cell_size + padding) + padding,
+                     _width * cell_size + padding * (_width - 1),
+                     _height * cell_size + padding * (_height - 1));
+}
+
+auto user_interface::init() -> void
+{
+    const Font font = LoadFontEx(FONT, FONT_SIZE, nullptr, 0);
+    SetTextureFilter(font.texture, TEXTURE_FILTER_BILINEAR);
+    GuiSetFont(font);
+
+    default_style style = get_default_style();
+    style.text_size = FONT_SIZE;
+    apply_color(style, GRAY);
+
+    set_default_style(style);
+}
+
+auto user_interface::apply_color(style& style, const Color color) -> void
+{
+    style.base_color_normal = ColorToInt(Fade(color, 0.8));
+    style.base_color_focused = ColorToInt(Fade(color, 0.3));
+    style.base_color_pressed = ColorToInt(Fade(color, 0.8));
+    style.base_color_disabled = ColorToInt(Fade(color, 0.5));
+
+    style.border_color_normal = ColorToInt(Fade(color, 1.0));
+    style.border_color_focused = ColorToInt(Fade(color, 0.7));
+    style.border_color_pressed = ColorToInt(Fade(color, 1.0));
+    style.border_color_disabled = ColorToInt(Fade(GRAY, 0.5));
+
+    style.text_color_normal = ColorToInt(Fade(BLACK, 1.0));
+    style.text_color_focused = ColorToInt(Fade(BLACK, 1.0));
+    style.text_color_pressed = ColorToInt(Fade(BLACK, 1.0));
+    style.text_color_disabled = ColorToInt(Fade(BLACK, 0.5));
+}
+
+auto user_interface::apply_block_color(style& style, const Color color) -> void
+{
+    style.base_color_normal = ColorToInt(Fade(color, 0.5));
+    style.base_color_focused = ColorToInt(Fade(color, 0.3));
+    style.base_color_pressed = ColorToInt(Fade(color, 0.8));
+    style.base_color_disabled = ColorToInt(Fade(color, 0.5));
+
+    style.border_color_normal = ColorToInt(Fade(color, 1.0));
+    style.border_color_focused = ColorToInt(Fade(color, 0.7));
+    style.border_color_pressed = ColorToInt(Fade(color, 1.0));
+    style.border_color_disabled = ColorToInt(Fade(GRAY, 0.5));
+}
+
+auto user_interface::apply_text_color(style& style, const Color color) -> void
+{
+    style.text_color_normal = ColorToInt(Fade(color, 1.0));
+    style.text_color_focused = ColorToInt(Fade(color, 1.0));
+    style.text_color_pressed = ColorToInt(Fade(color, 1.0));
+    style.text_color_disabled = ColorToInt(Fade(BLACK, 0.5));
+}
+
+auto user_interface::get_default_style() -> default_style
+{
+    // Could've iterated over the values, but then it wouldn't be as nice to
+    // access...
+    return {
+        {
+            GuiGetStyle(DEFAULT, BORDER_COLOR_NORMAL),
+            GuiGetStyle(DEFAULT, BASE_COLOR_NORMAL),
+            GuiGetStyle(DEFAULT, TEXT_COLOR_NORMAL),
+            GuiGetStyle(DEFAULT, BORDER_COLOR_FOCUSED),
+            GuiGetStyle(DEFAULT, BASE_COLOR_FOCUSED),
+            GuiGetStyle(DEFAULT, TEXT_COLOR_FOCUSED),
+            GuiGetStyle(DEFAULT, BORDER_COLOR_PRESSED),
+            GuiGetStyle(DEFAULT, BASE_COLOR_PRESSED),
+            GuiGetStyle(DEFAULT, TEXT_COLOR_PRESSED),
+            GuiGetStyle(DEFAULT, BORDER_COLOR_DISABLED),
+            GuiGetStyle(DEFAULT, BASE_COLOR_DISABLED),
+            GuiGetStyle(DEFAULT, TEXT_COLOR_DISABLED)
+        },
+        GuiGetStyle(DEFAULT, BACKGROUND_COLOR),
+        GuiGetStyle(DEFAULT, LINE_COLOR),
+        GuiGetStyle(DEFAULT, TEXT_SIZE),
+        GuiGetStyle(DEFAULT, TEXT_SPACING),
+        GuiGetStyle(DEFAULT, TEXT_LINE_SPACING),
+        GuiGetStyle(DEFAULT, TEXT_ALIGNMENT_VERTICAL),
+        GuiGetStyle(DEFAULT, TEXT_WRAP_MODE)
+    };
+}
+
+auto user_interface::set_default_style(const default_style& style) -> void
+{
+    GuiSetStyle(DEFAULT, BORDER_COLOR_NORMAL, style.border_color_normal);
+    GuiSetStyle(DEFAULT, BASE_COLOR_NORMAL, style.base_color_normal);
+    GuiSetStyle(DEFAULT, TEXT_COLOR_NORMAL, style.text_color_normal);
+
+    GuiSetStyle(DEFAULT, BORDER_COLOR_FOCUSED, style.border_color_focused);
+    GuiSetStyle(DEFAULT, BASE_COLOR_FOCUSED, style.base_color_focused);
+    GuiSetStyle(DEFAULT, TEXT_COLOR_FOCUSED, style.text_color_focused);
+
+    GuiSetStyle(DEFAULT, BORDER_COLOR_PRESSED, style.border_color_pressed);
+    GuiSetStyle(DEFAULT, BASE_COLOR_PRESSED, style.base_color_pressed);
+    GuiSetStyle(DEFAULT, TEXT_COLOR_PRESSED, style.text_color_pressed);
+
+    GuiSetStyle(DEFAULT, BORDER_COLOR_DISABLED, style.border_color_disabled);
+    GuiSetStyle(DEFAULT, BASE_COLOR_DISABLED, style.base_color_disabled);
+    GuiSetStyle(DEFAULT, TEXT_COLOR_DISABLED, style.text_color_disabled);
+
+    GuiSetStyle(DEFAULT, BACKGROUND_COLOR, style.background_color);
+    GuiSetStyle(DEFAULT, LINE_COLOR, style.line_color);
+
+    GuiSetStyle(DEFAULT, TEXT_SIZE, style.text_size);
+    GuiSetStyle(DEFAULT, TEXT_SPACING, style.text_spacing);
+    GuiSetStyle(DEFAULT, TEXT_LINE_SPACING, style.text_line_spacing);
+    GuiSetStyle(DEFAULT, TEXT_ALIGNMENT_VERTICAL, style.text_alignment_vertical);
+    GuiSetStyle(DEFAULT, TEXT_WRAP_MODE, style.text_wrap_mode);
+}
+
+auto user_interface::get_component_style(const int component) -> component_style
+{
+    return {
+        {
+            GuiGetStyle(component, BORDER_COLOR_NORMAL),
+            GuiGetStyle(component, BASE_COLOR_NORMAL),
+            GuiGetStyle(component, TEXT_COLOR_NORMAL),
+            GuiGetStyle(component, BORDER_COLOR_FOCUSED),
+            GuiGetStyle(component, BASE_COLOR_FOCUSED),
+            GuiGetStyle(component, TEXT_COLOR_FOCUSED),
+            GuiGetStyle(component, BORDER_COLOR_PRESSED),
+            GuiGetStyle(component, BASE_COLOR_PRESSED),
+            GuiGetStyle(component, TEXT_COLOR_PRESSED),
+            GuiGetStyle(component, BORDER_COLOR_DISABLED),
+            GuiGetStyle(component, BASE_COLOR_DISABLED),
+            GuiGetStyle(component, TEXT_COLOR_DISABLED)
+        },
+        GuiGetStyle(component, BORDER_WIDTH),
+        GuiGetStyle(component, TEXT_PADDING),
+        GuiGetStyle(component, TEXT_ALIGNMENT)
+    };
+}
+
+auto user_interface::set_component_style(const int component, const component_style& style) -> void
+{
+    GuiSetStyle(component, BORDER_COLOR_NORMAL, style.border_color_normal);
+    GuiSetStyle(component, BASE_COLOR_NORMAL, style.base_color_normal);
+    GuiSetStyle(component, TEXT_COLOR_NORMAL, style.text_color_normal);
+
+    GuiSetStyle(component, BORDER_COLOR_FOCUSED, style.border_color_focused);
+    GuiSetStyle(component, BASE_COLOR_FOCUSED, style.base_color_focused);
+    GuiSetStyle(component, TEXT_COLOR_FOCUSED, style.text_color_focused);
+
+    GuiSetStyle(component, BORDER_COLOR_PRESSED, style.border_color_pressed);
+    GuiSetStyle(component, BASE_COLOR_PRESSED, style.base_color_pressed);
+    GuiSetStyle(component, TEXT_COLOR_PRESSED, style.text_color_pressed);
+
+    GuiSetStyle(component, BORDER_COLOR_DISABLED, style.border_color_disabled);
+    GuiSetStyle(component, BASE_COLOR_DISABLED, style.base_color_disabled);
+    GuiSetStyle(component, TEXT_COLOR_DISABLED, style.text_color_disabled);
+
+    GuiSetStyle(component, BORDER_WIDTH, style.border_width);
+    GuiSetStyle(component, TEXT_PADDING, style.text_padding);
+    GuiSetStyle(component, TEXT_ALIGNMENT, style.text_alignment);
+}
+
+auto user_interface::popup_bounds() -> Rectangle
+{
+    return Rectangle(static_cast<float>(GetScreenWidth()) / 2.0f - POPUP_WIDTH / 2.0f,
+                     static_cast<float>(GetScreenHeight()) / 2.0f - POPUP_HEIGHT / 2.0f,
+                     POPUP_WIDTH,
+                     POPUP_HEIGHT);
+}
+
+auto user_interface::draw_button(const Rectangle bounds,
+                                 const std::string& label,
+                                 const Color color,
+                                 const bool enabled,
+                                 const int font_size) const -> int
+{
+    // Save original styling
+    const default_style original_default = get_default_style();
+    const component_style original_button = get_component_style(BUTTON);
+
+    // Change styling
+    default_style style_default = original_default;
+    component_style style_button = original_button;
+    style_default.text_size = font_size;
+    apply_color(style_button, color);
+    set_default_style(style_default);
+    set_component_style(BUTTON, style_button);
+
+    const int _state = GuiGetState();
+    if (!enabled || window_open()) {
+        GuiSetState(STATE_DISABLED);
+    }
+    const int pressed = GuiButton(bounds, label.data());
+    if (!enabled || window_open()) {
+        GuiSetState(_state);
+    }
+
+    // Restore original styling
+    set_default_style(original_default);
+    set_component_style(BUTTON, original_button);
+
+    return pressed;
+}
+
+auto user_interface::draw_menu_button(const int x,
+                                      const int y,
+                                      const int width,
+                                      const int height,
+                                      const std::string& label,
+                                      const Color color,
+                                      const bool enabled,
+                                      const int font_size) const -> int
+{
+    const Rectangle bounds = menu_grid.bounds(x, y, width, height);
+    return draw_button(bounds, label, color, enabled, font_size);
+}
+
+auto user_interface::draw_toggle_slider(const Rectangle bounds,
+                                        const std::string& off_label,
+                                        const std::string& on_label,
+                                        int* active,
+                                        Color color,
+                                        bool enabled,
+                                        int font_size) const -> int
+{
+    // Save original styling
+    const default_style original_default = get_default_style();
+    const component_style original_slider = get_component_style(SLIDER);
+    const component_style original_toggle = get_component_style(TOGGLE);
+
+    // Change styling
+    default_style style_default = original_default;
+    component_style style_slider = original_slider;
+    component_style style_toggle = original_toggle;
+    style_default.text_size = font_size;
+    apply_color(style_slider, color);
+    apply_color(style_toggle, color);
+    set_default_style(style_default);
+    set_component_style(SLIDER, style_slider);
+    set_component_style(TOGGLE, style_toggle);
+
+    const int _state = GuiGetState();
+    if (!enabled || window_open()) {
+        GuiSetState(STATE_DISABLED);
+    }
+    int pressed = GuiToggleSlider(bounds, std::format("{};{}", off_label, on_label).data(), active);
+    if (!enabled || window_open()) {
+        GuiSetState(_state);
+    }
+
+    // Restore original styling
+    set_default_style(original_default);
+    set_component_style(SLIDER, original_slider);
+    set_component_style(TOGGLE, original_toggle);
+
+    return pressed;
+}
+
+auto user_interface::draw_menu_toggle_slider(const int x,
+                                             const int y,
+                                             const int width,
+                                             const int height,
+                                             const std::string& off_label,
+                                             const std::string& on_label,
+                                             int* active,
+                                             const Color color,
+                                             const bool enabled,
+                                             const int font_size) const -> int
+{
+    const Rectangle bounds = menu_grid.bounds(x, y, width, height);
+    return draw_toggle_slider(bounds, off_label, on_label, active, color, enabled, font_size);
+}
+
+auto user_interface::draw_spinner(Rectangle bounds,
+                                  const std::string& label,
+                                  int* value,
+                                  int min,
+                                  int max,
+                                  Color color,
+                                  bool enabled,
+                                  int font_size) const -> int
+{
+    // Save original styling
+    const default_style original_default = get_default_style();
+    const component_style original_valuebox = get_component_style(VALUEBOX);
+    const component_style original_button = get_component_style(BUTTON);
+
+    // Change styling
+    default_style style_default = original_default;
+    component_style style_valuebox = original_valuebox;
+    component_style style_button = original_button;
+    style_default.text_size = font_size;
+    apply_color(style_valuebox, color);
+    apply_color(style_button, color);
+    set_default_style(style_default);
+    set_component_style(VALUEBOX, style_valuebox);
+    set_component_style(BUTTON, style_button);
+
+    const int _state = GuiGetState();
+    if (!enabled || window_open()) {
+        GuiSetState(STATE_DISABLED);
+    }
+    int pressed = GuiSpinner(bounds, "", label.data(), value, min, max, false);
+    if (!enabled || window_open()) {
+        GuiSetState(_state);
+    }
+
+    // Restore original styling
+    set_default_style(original_default);
+    set_component_style(VALUEBOX, original_valuebox);
+    set_component_style(BUTTON, style_button);
+
+    return pressed;
+}
+
+auto user_interface::draw_menu_spinner(const int x,
+                                       const int y,
+                                       const int width,
+                                       const int height,
+                                       const std::string& label,
+                                       int* value,
+                                       const int min,
+                                       const int max,
+                                       const Color color,
+                                       const bool enabled,
+                                       const int font_size) const -> int
+{
+    const Rectangle bounds = menu_grid.bounds(x, y, width, height);
+    return draw_spinner(bounds, label, value, min, max, color, enabled, font_size);
+}
+
+auto user_interface::draw_label(const Rectangle bounds,
+                                const std::string& text,
+                                const Color color,
+                                const bool enabled,
+                                const int font_size) const -> int
+{
+    // Save original styling
+    const default_style original_default = get_default_style();
+    const component_style original_label = get_component_style(LABEL);
+
+    // Change styling
+    default_style style_default = original_default;
+    component_style style_label = original_label;
+    style_default.text_size = font_size;
+    apply_text_color(style_label, color);
+    set_default_style(style_default);
+    set_component_style(LABEL, style_label);
+
+    const int _state = GuiGetState();
+    if (!enabled || window_open()) {
+        GuiSetState(STATE_DISABLED);
+    }
+    const int pressed = GuiLabel(bounds, text.data());
+    if (!enabled || window_open()) {
+        GuiSetState(_state);
+    }
+
+    // Restore original styling
+    set_default_style(original_default);
+    set_component_style(LABEL, original_label);
+
+    return pressed;
+}
+
+auto user_interface::draw_board_block(const int x,
+                                      const int y,
+                                      const int width,
+                                      const int height,
+                                      const Color color,
+                                      const bool enabled) const -> bool
+{
+    component_style s = get_component_style(BUTTON);
+    apply_block_color(s, color);
+
+    const Rectangle bounds = board_grid.square_bounds(x, y, width, height);
+
+    const bool focused = CheckCollisionPointRec(input.mouse - Vector2(0, MENU_HEIGHT), bounds);
+    const bool pressed = block(x, y, width, height, false).covers(input.sel_x, input.sel_y);
+
+    // Background to make faded colors work
+    DrawRectangleRec(bounds, RAYWHITE);
+
+    Color base = GetColor(s.base_color_normal);
+    Color border = GetColor(s.base_color_normal);
+    if (pressed) {
+        base = GetColor(s.base_color_pressed);
+        border = GetColor(s.base_color_pressed);
+    }
+    if (focused) {
+        base = GetColor(s.base_color_focused);
+        border = GetColor(s.base_color_focused);
+    }
+    if (focused && IsMouseButtonDown(MOUSE_BUTTON_LEFT)) {
+        base = GetColor(s.base_color_pressed);
+        border = GetColor(s.base_color_pressed);
+    }
+    if (!enabled) {
+        base = BOARD_COLOR_RESTRICTED;
+    }
+    DrawRectangleRec(bounds, base);
+    if (enabled) {
+        DrawRectangleLinesEx(bounds, 2.0, border);
+    }
+
+    return focused && enabled;
+}
+
+auto user_interface::window_open() const -> bool
+{
+    return save_window || help_window || ok_message || yes_no_message;
+}
+
+auto user_interface::draw_menu_header(const Color color) const -> void
+{
+    int preset = static_cast<int>(state.get_current_preset());
+    draw_menu_spinner(0,
+                      0,
+                      1,
+                      1,
+                      "Preset: ",
+                      &preset,
+                      -1,
+                      static_cast<int>(state.get_preset_count()),
+                      color,
+                      !input.editing);
+    if (preset > static_cast<int>(state.get_current_preset())) {
+        input.load_next_preset();
+    } else if (preset < static_cast<int>(state.get_current_preset())) {
+        input.load_previous_preset();
+    }
+
+    draw_menu_button(1,
+                     0,
+                     1,
+                     1,
+                     std::format("{}: {}/{} Blocks",
+                                 state.was_edited()
+                                     ? "Modified"
+                                     : std::format("\"{}\"", state.get_current_preset_comment().substr(2)),
+                                 state.get_current_state().block_count(),
+                                 puzzle::MAX_BLOCKS),
+                     color);
+
+    int editing = input.editing;
+    draw_menu_toggle_slider(2, 0, 1, 1, "Puzzle Mode (Tab)", "Edit Mode (Tab)", &editing, color);
+    if (editing != input.editing) {
+        input.toggle_editing();
+    }
+}
+
+auto user_interface::draw_graph_info(const Color color) const -> void
+{
+    draw_menu_button(0,
+                     1,
+                     1,
+                     1,
+                     std::format("Found {} States ({} Winning)", state.get_state_count(), state.get_target_count()),
+                     color);
+
+    draw_menu_button(1, 1, 1, 1, std::format("Found {} Transitions", state.get_link_count()), color);
+
+    draw_menu_button(2,
+                     1,
+                     1,
+                     1,
+                     std::format("{} Moves to Nearest Solution",
+                                 state.get_path_length() > 0 ? state.get_path_length() - 1 : 0),
+                     color);
+}
+
+auto user_interface::draw_graph_controls(const Color color) const -> void
+{
+    if (draw_menu_button(0, 2, 1, 1, "Populate Graph (G)", color)) {
+        input.populate_graph();
+    }
+
+    // int mark_path = input.mark_path;
+    // DrawMenuToggleSlider(2, 2, 1, 1, "Path Hidden (U)", "Path Shown (U)",
+    //                      &mark_path, color);
+    // if (mark_path != input.mark_path) {
+    //   input.ToggleMarkPath();
+    // }
+
+    if (draw_menu_button(1, 2, 1, 1, "Clear Graph (C)", color)) {
+        input.clear_graph();
+    }
+
+    int mark_solutions = input.mark_solutions;
+    draw_menu_toggle_slider(2, 2, 1, 1, "Solution Hidden (I)", "Solution Shown (I)", &mark_solutions, color);
+    if (mark_solutions != input.mark_solutions) {
+        input.toggle_mark_solutions();
+    }
+    input.mark_path = input.mark_solutions;
+}
+
+auto user_interface::draw_camera_controls(const Color color) const -> void
+{
+    int lock_camera = input.camera_lock;
+    draw_menu_toggle_slider(0, 3, 1, 1, "Free Camera (L)", "Locked Camera (L)", &lock_camera, color);
+    if (lock_camera != input.camera_lock) {
+        input.toggle_camera_lock();
+    }
+
+    int lock_camera_mass_center = input.camera_mass_center_lock;
+    draw_menu_toggle_slider(1,
+                            3,
+                            1,
+                            1,
+                            "Current Block (U)",
+                            "Graph Center (U)",
+                            &lock_camera_mass_center,
+                            color,
+                            input.camera_lock);
+    if (lock_camera_mass_center != input.camera_mass_center_lock) {
+        input.toggle_camera_mass_center_lock();
+    }
+
+    int projection = camera.projection == CAMERA_ORTHOGRAPHIC;
+    draw_menu_toggle_slider(2, 3, 1, 1, "Perspective (Alt)", "Orthographic (Alt)", &projection, color);
+    if (projection != (camera.projection == CAMERA_ORTHOGRAPHIC)) {
+        input.toggle_camera_projection();
+    }
+}
+
+auto user_interface::draw_puzzle_controls(const Color color) const -> void
+{
+    auto nth = [&](const int n)
+    {
+        if (n == 11 || n == 12 || n == 13) {
+            return "th";
+        }
+        if (n % 10 == 1) {
+            return "st";
+        }
+        if (n % 10 == 2) {
+            return "nd";
+        }
+        if (n % 10 == 3) {
+            return "rd";
+        }
+        return "th";
+    };
+
+    const int visits = state.get_current_visits();
+    draw_menu_button(0,
+                     4,
+                     1,
+                     1,
+                     std::format("{} Moves ({}{} Time at this State)", state.get_total_moves(), visits, nth(visits)),
+                     color);
+
+    if (draw_menu_button(1, 4, 1, 1, "Make Optimal Move (Space)", color, state.has_distances())) {
+        input.goto_optimal_next_state();
+    }
+
+    if (draw_menu_button(2, 4, 1, 1, "Undo Last Move (Backspace)", color, state.has_history())) {
+        input.goto_previous_state();
+    }
+
+    if (draw_menu_button(0, 5, 1, 1, "Go to Nearest Solution (B)", color, state.has_distances())) {
+        input.goto_closest_target_state();
+    }
+
+    if (draw_menu_button(1, 5, 1, 1, "Go to Worst State (V)", color, state.has_distances())) {
+        input.goto_most_distant_state();
+    }
+
+    if (draw_menu_button(2,
+                         5,
+                         1,
+                         1,
+                         "Go to Starting State (R)",
+                         color,
+                         state.get_current_index() != state.get_starting_index())) {
+        input.goto_starting_state();
+    }
+}
+
+auto user_interface::draw_edit_controls(const Color color) const -> void
+{
+    const puzzle& current = state.get_current_state();
+
+    // Toggle Target Block
+    if (draw_menu_button(0, 4, 1, 1, "Toggle Target Block (T)", color)) {
+        input.toggle_target_block();
+    }
+
+    // Toggle Wall Block
+    if (draw_menu_button(0, 5, 1, 1, "Toggle Wall Block (Y)", color)) {
+        input.toggle_wall_block();
+    }
+
+    // Toggle Restricted/Free Block Movement
+    int free = !current.get_restricted();
+    draw_menu_toggle_slider(1, 4, 1, 1, "Restricted (F)", "Free (F)", &free, color);
+    if (free != !current.get_restricted()) {
+        input.toggle_restricted_movement();
+    }
+
+    // Clear Goal
+    if (draw_menu_button(1, 5, 1, 1, "Clear Goal (X)", color)) {}
+
+    // Column Count Spinner
+    int columns = current.get_width();
+    draw_menu_spinner(2, 4, 1, 1, "Cols: ", &columns, puzzle::MIN_WIDTH, puzzle::MAX_WIDTH, color);
+    if (columns > current.get_width()) {
+        input.add_board_column();
+    } else if (columns < current.get_width()) {
+        input.remove_board_column();
+    }
+
+    // Row Count Spinner
+    int rows = current.get_height();
+    draw_menu_spinner(2, 5, 1, 1, "Rows: ", &rows, puzzle::MIN_WIDTH, puzzle::MAX_WIDTH, color);
+    if (rows > current.get_height()) {
+        input.add_board_row();
+    } else if (rows < current.get_height()) {
+        input.remove_board_row();
+    }
+}
+
+auto user_interface::draw_menu_footer(const Color color) -> void
+{
+    draw_menu_button(0, 6, 2, 1, state.get_current_state().string_repr().data(), color);
+
+    if (draw_menu_button(2, 6, 1, 1, "Save as Preset", color)) {
+        if (const std::optional<std::string>& reason = state.get_current_state().try_get_invalid_reason()) {
+            message_title = "Can't Save Preset";
+            message_message = std::format("Invalid Board: {}.", *reason);
+            ok_message = true;
+        } else {
+            save_window = true;
+        }
+    }
+}
+
+auto user_interface::get_background_color() -> Color
+{
+    return GetColor(GuiGetStyle(DEFAULT, BACKGROUND_COLOR));
+}
+
+auto user_interface::help_popup() -> void {}
+
+auto user_interface::draw_save_preset_popup() -> void
+{
+    if (!save_window) {
+        return;
+    }
+
+    // Returns the pressed button index
+    const int button = GuiTextInputBox(popup_bounds(),
+                                       "Save as Preset",
+                                       "Enter Preset Name",
+                                       "Ok;Cancel",
+                                       preset_comment.data(),
+                                       255,
+                                       nullptr);
+    if (button == 1) {
+        state.save_current_to_preset_file(preset_comment.data());
+    }
+    if (button == 0 || button == 1 || button == 2) {
+        save_window = false;
+        TextCopy(preset_comment.data(), "\0");
+    }
+}
+
+auto user_interface::draw_ok_message_box() -> void
+{
+    if (!ok_message) {
+        return;
+    }
+
+    const int button = GuiMessageBox(popup_bounds(), message_title.data(), message_message.data(), "Ok");
+    if (button == 0 || button == 1) {
+        message_title = "";
+        message_message = "";
+        ok_message = false;
+    }
+}
+
+auto user_interface::draw_yes_no_message_box() -> void
+{
+    if (!yes_no_message) {
+        return;
+    }
+
+    const int button = GuiMessageBox(popup_bounds(), message_title.data(), message_message.data(), "Yes;No");
+    if (button == 1) {
+        yes_no_handler();
+    }
+    if (button == 0 || button == 1 || button == 2) {
+        message_title = "";
+        message_message = "";
+        yes_no_message = false;
+    }
+}
+
+auto user_interface::draw_main_menu() -> void
+{
+    menu_grid.update_bounds(0, 0, GetScreenWidth(), MENU_HEIGHT);
+
+    draw_menu_header(GRAY);
+    draw_graph_info(ORANGE);
+    draw_graph_controls(RED);
+    draw_camera_controls(DARKGREEN);
+
+    if (input.editing) {
+        draw_edit_controls(PURPLE);
+    } else {
+        draw_puzzle_controls(BLUE);
+    }
+
+    draw_menu_footer(GRAY);
+}
+
+auto user_interface::draw_puzzle_board() -> void
+{
+    const puzzle& current = state.get_current_state();
+
+    board_grid.update_bounds(0,
+                             MENU_HEIGHT,
+                             GetScreenWidth() / 2,
+                             GetScreenHeight() - MENU_HEIGHT,
+                             current.get_width(),
+                             current.get_height());
+
+    // Draw outer border
+    const Rectangle bounds = board_grid.square_bounds();
+    DrawRectangleRec(bounds, current.goal_reached() ? BOARD_COLOR_WON : BOARD_COLOR_RESTRICTED);
+
+    // Draw inner borders
+    DrawRectangle(bounds.x + BOARD_PADDING,
+                  bounds.y + BOARD_PADDING,
+                  bounds.width - 2 * BOARD_PADDING,
+                  bounds.height - 2 * BOARD_PADDING,
+                  current.get_restricted() ? BOARD_COLOR_RESTRICTED : BOARD_COLOR_FREE);
+
+    // Draw target opening
+    // TODO: Only draw single direction (in corner) if restricted (use target block principal
+    // direction)
+    const std::optional<block> target_block = current.try_get_target_block();
+    const int target_x = current.get_goal_x();
+    const int target_y = current.get_goal_y();
+    if (current.get_goal() && target_block) {
+        auto [x, y, width, height] = board_grid.square_bounds(target_x,
+                                                              target_y,
+                                                              target_block->get_width(),
+                                                              target_block->get_height());
+
+        const Color opening_color = Fade(current.goal_reached() ? BOARD_COLOR_WON : BOARD_COLOR_RESTRICTED, 0.3);
+
+        if (target_x == 0) {
+            // Left opening
+            DrawRectangle(x - BOARD_PADDING, y, BOARD_PADDING, height, RAYWHITE);
+            DrawRectangle(x - BOARD_PADDING, y, BOARD_PADDING, height, opening_color);
+        }
+        if (target_x + target_block->get_width() == current.get_width()) {
+            // Right opening
+            DrawRectangle(x + width, y, BOARD_PADDING, height, RAYWHITE);
+            DrawRectangle(x + width, y, BOARD_PADDING, height, opening_color);
+        }
+        if (target_y == 0) {
+            // Top opening
+            DrawRectangle(x, y - BOARD_PADDING, width, BOARD_PADDING, RAYWHITE);
+            DrawRectangle(x, y - BOARD_PADDING, width, BOARD_PADDING, opening_color);
+        }
+        if (target_y + target_block->get_height() == current.get_height()) {
+            // Bottom opening
+            DrawRectangle(x, y + height, width, BOARD_PADDING, RAYWHITE);
+            DrawRectangle(x, y + height, width, BOARD_PADDING, opening_color);
+        }
+    }
+
+    // Draw empty cells. Also set hovered blocks
+    input.hov_x = -1;
+    input.hov_y = -1;
+    for (int x = 0; x < board_grid.columns; ++x) {
+        for (int y = 0; y < board_grid.rows; ++y) {
+            DrawRectangleRec(board_grid.square_bounds(x, y, 1, 1), RAYWHITE);
+
+            Rectangle hov_bounds = board_grid.square_bounds(x, y, 1, 1);
+            hov_bounds.x -= BOARD_PADDING;
+            hov_bounds.y -= BOARD_PADDING;
+            hov_bounds.width += BOARD_PADDING;
+            hov_bounds.height += BOARD_PADDING;
+            if (CheckCollisionPointRec(GetMousePosition() - Vector2(0, MENU_HEIGHT), hov_bounds)) {
+                input.hov_x = x;
+                input.hov_y = y;
+            }
+        }
+    }
+
+    // Draw blocks
+    for (const block b : current.block_view()) {
+        Color c = BLOCK_COLOR;
+        if (b.get_target()) {
+            c = TARGET_BLOCK_COLOR;
+        } else if (b.get_immovable()) {
+            c = WALL_COLOR;
+        }
+
+        const auto [x, y, w, h, t, i] = b.unpack_repr();
+
+        draw_board_block(x, y, w, h, c, !i);
+    }
+
+    // Draw block placing
+    if (input.editing && input.has_block_add_xy) {
+        if (current.covers(input.block_add_x, input.block_add_y) && input.hov_x >= input.block_add_x && input.hov_y >=
+            input.block_add_y) {
+            bool collides = false;
+            for (const block b : current.block_view()) {
+                if (b.collides(block(input.block_add_x,
+                                     input.block_add_y,
+                                     input.hov_x - input.block_add_x + 1,
+                                     input.hov_y - input.block_add_y + 1,
+                                     false))) {
+                    collides = true;
+                    break;
+                }
+            }
+            if (!collides) {
+                draw_board_block(input.block_add_x,
+                                 input.block_add_y,
+                                 input.hov_x - input.block_add_x + 1,
+                                 input.hov_y - input.block_add_y + 1,
+                                 PURPLE);
+            }
+        }
+    }
+
+    // TODO: In edit mode
+    //       - Clear Goal button doesn't work
+    //       - Toggle Target Block button throws "Grid bounds out of range"
+    //       - Clicking the goal to remove it throws "Grid bounds out of range"
+    // Draw goal boundaries when editing
+    if (input.editing && current.get_goal() && target_block) {
+        DrawRectangleLinesEx(
+            board_grid.square_bounds(target_x, target_y, target_block->get_width(), target_block->get_height()),
+            2.0,
+            TARGET_BLOCK_COLOR);
+    }
+}
+
+auto user_interface::draw_graph_overlay(int fps, int ups, size_t mass_count, size_t spring_count) -> void
+{
+    graph_overlay_grid.update_bounds(GetScreenWidth() / 2, MENU_HEIGHT);
+    debug_overlay_grid.update_bounds(GetScreenWidth() / 2, GetScreenHeight() - 75);
+
+    draw_label(graph_overlay_grid.bounds(0, 0, 1, 1), std::format("Dist: {:0>7.2f}", camera.distance), BLACK);
+    draw_label(graph_overlay_grid.bounds(0, 1, 1, 1), std::format("FoV:  {:0>6.2f}", camera.fov), BLACK);
+    draw_label(graph_overlay_grid.bounds(0, 2, 1, 1), std::format("FPS:  {:0>3}", fps), LIME);
+    draw_label(graph_overlay_grid.bounds(0, 3, 1, 1), std::format("UPS:  {:0>3}", ups), ORANGE);
+
+    // Debug
+    draw_label(debug_overlay_grid.bounds(0, 0, 1, 1), std::format("Physics Debug:"), BLACK);
+    draw_label(debug_overlay_grid.bounds(0, 1, 1, 1), std::format("Masses:  {}", mass_count), BLACK);
+    draw_label(debug_overlay_grid.bounds(0, 2, 1, 1), std::format("Springs: {}", spring_count), BLACK);
+}
+
+auto user_interface::draw(const int fps, const int ups, const size_t mass_count, const size_t spring_count) -> void
+{
+    const auto visitor = overloads{
+        [&](const show_ok_message& msg)
+        {
+            message_title = msg.title;
+            message_message = msg.message;
+            ok_message = true;
+        },
+        [&](const show_yes_no_message& msg)
+        {
+            message_title = msg.title;
+            message_message = msg.message;
+            yes_no_handler = msg.on_yes;
+            yes_no_message = true;
+        },
+        [&](const show_save_preset_window& msg)
+        {
+            save_window = true;
+        }
+    };
+
+    while (!input.ui_commands.empty()) {
+        const ui_command& cmd = input.ui_commands.front();
+
+        cmd.visit(visitor);
+
+        input.ui_commands.pop();
+    }
+
+    input.disable = window_open();
+
+    draw_graph_overlay(fps, ups, mass_count, spring_count);
+    draw_save_preset_popup();
+    draw_ok_message_box();
+    draw_yes_no_message_box();
+}

test/bitmap.cpp

@@ -0,0 +1,74 @@
+// ReSharper disable CppLocalVariableMayBeConst
+#include "puzzle.hpp"
+
+#include <random>
+#include <catch2/catch_test_macros.hpp>
+
+TEST_CASE("bitmap_is_full all bits set", "[puzzle][board]")
+{
+    puzzle p1(5, 5);
+    puzzle p2(3, 4);
+    puzzle p3(5, 4);
+    puzzle p4(3, 7);
+    u64 bitmap = -1;
+
+    REQUIRE(p1.bitmap_is_full(bitmap));
+    REQUIRE(p2.bitmap_is_full(bitmap));
+    REQUIRE(p3.bitmap_is_full(bitmap));
+    REQUIRE(p4.bitmap_is_full(bitmap));
+}
+
+TEST_CASE("bitmap_is_full no bits set", "[puzzle][board]")
+{
+    puzzle p1(5, 5);
+    puzzle p2(3, 4);
+    puzzle p3(5, 4);
+    puzzle p4(3, 7);
+    u64 bitmap = 0;
+
+    REQUIRE_FALSE(p1.bitmap_is_full(bitmap));
+    REQUIRE_FALSE(p2.bitmap_is_full(bitmap));
+    REQUIRE_FALSE(p3.bitmap_is_full(bitmap));
+    REQUIRE_FALSE(p4.bitmap_is_full(bitmap));
+}
+
+TEST_CASE("bitmap_is_full necessary bits set", "[puzzle][board]")
+{
+    puzzle p1(5, 5);
+    puzzle p2(3, 4);
+    puzzle p3(5, 4);
+    puzzle p4(3, 7);
+
+    u64 bitmap1 = (1ull << 25) - 1; // 5 * 5
+    u64 bitmap2 = (1ull << 12) - 1;  // 3 * 4
+    u64 bitmap3 = (1ull << 20) - 1; // 5 * 4
+    u64 bitmap4 = (1ull << 21) - 1; // 3 * 7
+
+    REQUIRE(p1.bitmap_is_full(bitmap1));
+    REQUIRE(p2.bitmap_is_full(bitmap2));
+    REQUIRE(p3.bitmap_is_full(bitmap3));
+    REQUIRE(p4.bitmap_is_full(bitmap4));
+}
+
+TEST_CASE("bitmap_is_full necessary bits not set", "[puzzle][board]")
+{
+    puzzle p1(5, 5);
+    puzzle p2(3, 4);
+    puzzle p3(5, 4);
+    puzzle p4(3, 7);
+
+    u64 bitmap1 = (1ull << 25) - 1; // 5 * 5
+    u64 bitmap2 = (1ull << 12) - 1;  // 3 * 4
+    u64 bitmap3 = (1ull << 20) - 1; // 5 * 4
+    u64 bitmap4 = (1ull << 21) - 1; // 3 * 7
+
+    bitmap1 &= ~(1ull << 12);
+    bitmap2 &= ~(1ull << 6);
+    bitmap3 &= ~(1ull << 8);
+    bitmap4 &= ~(1ull << 18);
+
+    REQUIRE_FALSE(p1.bitmap_is_full(bitmap1));
+    REQUIRE_FALSE(p2.bitmap_is_full(bitmap2));
+    REQUIRE_FALSE(p3.bitmap_is_full(bitmap3));
+    REQUIRE_FALSE(p4.bitmap_is_full(bitmap4));
+}

test/bitmap_find_first_empty.cpp

@@ -0,0 +1,266 @@
+// ReSharper disable CppLocalVariableMayBeConst
+#include "puzzle.hpp"
+
+#include <random>
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/generators/catch_generators.hpp>
+
+static auto board_mask(const int w, const int h) -> u64
+{
+    const int cells = w * h;
+    if (cells == 64) {
+        return ~0ULL;
+    }
+    return (1ULL << cells) - 1ULL;
+}
+
+TEST_CASE("Empty board returns (0,0)", "[puzzle][board]")
+{
+    puzzle p(5, 5);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(0ULL, x, y));
+
+    REQUIRE(x == 0);
+    REQUIRE(y == 0);
+}
+
+TEST_CASE("Full board detection respects width*height only", "[puzzle][board]")
+{
+    auto [w, h] = GENERATE(std::tuple{3, 3}, std::tuple{4, 4}, std::tuple{5, 6}, std::tuple{8, 8});
+
+    puzzle p(w, h);
+
+    u64 mask = board_mask(w, h);
+
+    int x = -1, y = -1;
+
+    REQUIRE_FALSE(p.bitmap_find_first_empty(mask, x, y));
+
+    // Bits outside board should not affect fullness
+    REQUIRE_FALSE(p.bitmap_find_first_empty(mask | (~mask), x, y));
+}
+
+TEST_CASE("Single empty cell at various positions", "[puzzle][board]")
+{
+    auto [w, h] = GENERATE(std::tuple{3, 3}, std::tuple{4, 4}, std::tuple{5, 5}, std::tuple{8, 8});
+
+    puzzle p(w, h);
+
+    int cells = w * h;
+
+    auto empty_index = GENERATE_COPY(values<int>({ 0, cells / 2, cells - 1}));
+
+    u64 bitmap = board_mask(w, h);
+    bitmap &= ~(1ULL << empty_index);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == empty_index % w);
+    REQUIRE(y == empty_index / w);
+}
+
+TEST_CASE("Bits outside board are ignored", "[puzzle][board]")
+{
+    puzzle p(3, 3); // 9 cells
+
+    u64 mask = board_mask(3, 3);
+
+    // Board is full
+    u64 bitmap = mask;
+
+    // Set extra bits outside 9 cells
+    bitmap |= (1ULL << 20);
+    bitmap |= (1ULL << 63);
+
+    int x = -1, y = -1;
+    REQUIRE_FALSE(p.bitmap_find_first_empty(bitmap, x, y));
+}
+
+TEST_CASE("First empty found in forward search branch", "[puzzle][branch]")
+{
+    puzzle p(4, 4); // 16 cells
+
+    // Only MSB (within board) set
+    u64 bitmap = (1ULL << 15);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == 0);
+    REQUIRE(y == 0);
+}
+
+TEST_CASE("Backward search branch finds gap before MSB cluster", "[puzzle][branch]")
+{
+    puzzle p(4, 4); // 16 cells
+
+    // Set bits 15,14,13 but leave 12 empty
+    u64 bitmap = (1ULL << 15) | (1ULL << 14) | (1ULL << 13);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == 0);
+    REQUIRE(y == 0);
+}
+
+TEST_CASE("Rectangular board coordinate mapping", "[puzzle][rect]")
+{
+    puzzle p(5, 3); // 15 cells
+
+    int empty_index = 11;
+
+    u64 bitmap = board_mask(5, 3);
+    bitmap &= ~(1ULL << empty_index);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == empty_index % 5);
+    REQUIRE(y == empty_index / 5);
+}
+
+TEST_CASE("Non-64-sized board near limit", "[puzzle][limit]")
+{
+    puzzle p(7, 8); // 56 cells
+
+    u64 mask = board_mask(7, 8);
+
+    // Full board should return false
+    int x = -1, y = -1;
+    REQUIRE_FALSE(p.bitmap_find_first_empty(mask, x, y));
+
+    // Clear highest valid cell
+    int empty_index = 55;
+    mask &= ~(1ULL << empty_index);
+
+    REQUIRE(p.bitmap_find_first_empty(mask, x, y));
+    REQUIRE(x == empty_index % 7);
+    REQUIRE(y == empty_index / 7);
+}
+
+// --- Oracle: find first zero bit inside board ---
+static auto oracle_find_first_empty(u64 bitmap, int w, int h, int& x, int& y) -> bool
+{
+    int cells = w * h;
+
+    for (int i = 0; i < cells; ++i) {
+        if ((bitmap & (1ULL << i)) == 0) {
+            x = i % w;
+            y = i / w;
+            return true;
+        }
+    }
+
+    return false;
+}
+
+TEST_CASE("Oracle validation across board sizes 3x3 to 8x8", "[puzzle][oracle]")
+{
+    auto [w, h] = GENERATE(std::tuple{3, 3}, std::tuple{4, 4}, std::tuple{5, 5}, std::tuple{6, 6}, std::tuple{7, 7},
+                           std::tuple{8, 8}, std::tuple{3, 5}, std::tuple{5, 3}, std::tuple{7, 8}, std::tuple{8, 7});
+
+    puzzle p(w, h);
+
+    u64 mask = board_mask(w, h);
+
+    std::mt19937_64 rng(12345);
+    std::uniform_int_distribution<u64> dist(0, UINT64_MAX);
+
+    for (int iteration = 0; iteration < 200; ++iteration) {
+        u64 bitmap = dist(rng);
+
+        int ox = -1, oy = -1;
+        bool oracle_result = oracle_find_first_empty(bitmap, w, h, ox, oy);
+
+        int x = -1, y = -1;
+        bool result = p.bitmap_find_first_empty(bitmap, x, y);
+
+        REQUIRE(result == oracle_result);
+
+        if (result) {
+            REQUIRE(x == ox);
+            REQUIRE(y == oy);
+        }
+    }
+}
+
+TEST_CASE("Bits set outside board only behaves as empty board", "[puzzle][outside]")
+{
+    puzzle p(3, 3); // 9 cells
+
+    u64 bitmap = (1ULL << 40) | (1ULL << 63);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == 0);
+    REQUIRE(y == 0);
+}
+
+TEST_CASE("Last valid cell empty stresses upper bound", "[puzzle][boundary]")
+{
+    auto [w, h] = GENERATE(std::tuple{4, 4}, std::tuple{5, 6}, std::tuple{7, 8}, std::tuple{8, 8});
+
+    puzzle p(w, h);
+
+    int cells = w * h;
+    u64 bitmap = board_mask(w, h);
+
+    // Clear last valid bit
+    bitmap &= ~(1ULL << (cells - 1));
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    REQUIRE(x == (cells - 1) % w);
+    REQUIRE(y == (cells - 1) / w);
+}
+
+TEST_CASE("Board sizes between 33 and 63 cells", "[puzzle][midrange]")
+{
+    auto [w, h] = GENERATE(std::tuple{6, 6}, // 36
+                           std::tuple{7, 6}, // 42
+                           std::tuple{7, 7}, // 49
+                           std::tuple{8, 7}, // 56
+                           std::tuple{7, 8}  // 56
+    );
+
+    puzzle p(w, h);
+
+    int cells = w * h;
+
+    for (int index : {31, 32, cells - 2}) {
+        if (index >= cells) continue;
+
+        u64 bitmap = board_mask(w, h);
+        bitmap &= ~(1ULL << index);
+
+        int x = -1, y = -1;
+        REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+        REQUIRE(x == index % w);
+        REQUIRE(y == index / w);
+    }
+}
+
+TEST_CASE("Multiple holes choose lowest index", "[puzzle][multiple]")
+{
+    puzzle p(5, 5);
+
+    u64 bitmap = board_mask(5, 5);
+
+    // Clear several positions
+    bitmap &= ~(1ULL << 3);
+    bitmap &= ~(1ULL << 7);
+    bitmap &= ~(1ULL << 12);
+
+    int x = -1, y = -1;
+    REQUIRE(p.bitmap_find_first_empty(bitmap, x, y));
+
+    // Oracle expectation: index 3
+    REQUIRE(x == 3 % 5);
+    REQUIRE(y == 3 / 5);
+}

test/bits.cpp

@@ -0,0 +1,267 @@
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/catch_template_test_macros.hpp>
+#include <cstdint>
+
+#include "bits.hpp"
+
+// =============================================================================
+// Catch2
+// =============================================================================
+//
+// 1. TEST_CASE(name, tags)
+//    The basic unit of testing in Catch2. Each TEST_CASE is an independent test
+//    function. The first argument is a descriptive name (must be unique), and
+//    the second is a string of tags in square brackets (e.g. "[set_bits]")
+//    used to filter and group tests when running.
+//
+// 2. SECTION(name)
+//    Sections allow multiple subtests within a single TEST_CASE. Each SECTION
+//    runs the TEST_CASE from the top, so any setup code before the SECTION is
+//    re-executed fresh for every section. This gives each section an isolated
+//    starting state without needing separate TEST_CASEs or explicit teardown.
+//    Sections can also be nested.
+//
+// 3. REQUIRE(expression)
+//    The primary assertion macro. If the expression evaluates to false, the
+//    test fails immediately and Catch2 reports the actual values of both sides
+//    of the comparison (e.g. "0xF5 == 0xF0" on failure). There is also
+//    CHECK(), which records a failure but continues executing the rest of the
+//    test; REQUIRE() aborts the current test on failure.
+//
+// 4. TEMPLATE_TEST_CASE(name, tags, Type1, Type2, ...)
+//    A parameterised test that is instantiated once for each type listed.
+//    Inside the test body, the alias `TestType` refers to the current type.
+//    This avoids duplicating identical logic for u8, u16, u32,
+//    and u64. Catch2 automatically appends the type name to the test name
+//    in the output so you can see which instantiation failed.
+//
+// 5. Tags (e.g. "[create_mask]", "[round-trip]")
+//    Tags let you selectively run subsets of tests from the command line.
+//    For example:
+//      ./tests "[set_bits]"        -- runs only tests tagged [set_bits]
+//      ./tests "~[round-trip]"     -- runs everything except [round-trip]
+//      ./tests "[get_bits],[set_bits]" -- runs tests matching either tag
+//
+// =============================================================================
+
+// ---------------------------------------------------------------------------
+// create_mask
+// ---------------------------------------------------------------------------
+
+TEMPLATE_TEST_CASE("create_mask produces correct masks", "[create_mask]",
+                   u8, u16, u32, u64)
+{
+    SECTION("single bit mask at bit 0") {
+        auto m = create_mask<TestType>(0, 0);
+        REQUIRE(m == TestType{0b1});
+    }
+
+    SECTION("single bit mask at bit 3") {
+        auto m = create_mask<TestType>(3, 3);
+        REQUIRE(m == TestType{0b1000});
+    }
+
+    SECTION("mask spanning bits 0..7 gives 0xFF") {
+        auto m = create_mask<TestType>(0, 7);
+        REQUIRE(m == TestType{0xFF});
+    }
+
+    SECTION("mask spanning bits 4..7") {
+        auto m = create_mask<TestType>(4, 7);
+        REQUIRE(m == TestType{0xF0});
+    }
+
+    SECTION("full-width mask returns all ones") {
+        constexpr u8 last = sizeof(TestType) * 8 - 1;
+        auto m = create_mask<TestType>(0, last);
+        REQUIRE(m == static_cast<TestType>(~TestType{0}));
+    }
+}
+
+TEST_CASE("create_mask 32-bit specific cases", "[create_mask]") {
+    REQUIRE(create_mask<u32>(0, 15)  == 0x0000FFFF);
+    REQUIRE(create_mask<u32>(0, 31)  == 0xFFFFFFFF);
+    REQUIRE(create_mask<u32>(16, 31) == 0xFFFF0000);
+}
+
+// ---------------------------------------------------------------------------
+// clear_bits
+// ---------------------------------------------------------------------------
+
+TEMPLATE_TEST_CASE("clear_bits zeroes the specified range", "[clear_bits]",
+                   u8, u16, u32, u64)
+{
+    SECTION("clear all bits") {
+        TestType val = static_cast<TestType>(~TestType{0});
+        constexpr u8 last = sizeof(TestType) * 8 - 1;
+        clear_bits(val, 0, last);
+        REQUIRE(val == TestType{0});
+    }
+
+    SECTION("clear lower nibble") {
+        TestType val = static_cast<TestType>(0xFF);
+        clear_bits(val, 0, 3);
+        REQUIRE(val == static_cast<TestType>(0xF0));
+    }
+
+    SECTION("clear upper nibble") {
+        TestType val = static_cast<TestType>(0xFF);
+        clear_bits(val, 4, 7);
+        REQUIRE(val == static_cast<TestType>(0x0F));
+    }
+
+    SECTION("clear single bit") {
+        TestType val = static_cast<TestType>(0xFF);
+        clear_bits(val, 0, 0);
+        REQUIRE(val == static_cast<TestType>(0xFE));
+    }
+
+    SECTION("clearing already-zero bits is a no-op") {
+        TestType val = TestType{0};
+        clear_bits(val, 0, 3);
+        REQUIRE(val == TestType{0});
+    }
+}
+
+// ---------------------------------------------------------------------------
+// set_bits
+// ---------------------------------------------------------------------------
+
+TEMPLATE_TEST_CASE("set_bits writes value into the specified range", "[set_bits]",
+                   u8, u16, u32, u64)
+{
+    SECTION("set lower nibble on zero") {
+        TestType val = TestType{0};
+        set_bits(val, u8{0}, u8{3}, static_cast<TestType>(0xA));
+        REQUIRE(val == static_cast<TestType>(0x0A));
+    }
+
+    SECTION("set upper nibble on zero") {
+        TestType val = TestType{0};
+        set_bits(val, u8{4}, u8{7}, static_cast<TestType>(0xB));
+        REQUIRE(val == static_cast<TestType>(0xB0));
+    }
+
+    SECTION("set_bits replaces existing bits") {
+        TestType val = static_cast<TestType>(0xFF);
+        set_bits(val, u8{0}, u8{3}, static_cast<TestType>(0x5));
+        REQUIRE(val == static_cast<TestType>(0xF5));
+    }
+
+    SECTION("set single bit to 1") {
+        TestType val = TestType{0};
+        set_bits(val, u8{3}, u8{3}, static_cast<TestType>(1));
+        REQUIRE(val == static_cast<TestType>(0x08));
+    }
+
+    SECTION("set single bit to 0") {
+        TestType val = static_cast<TestType>(0xFF);
+        set_bits(val, u8{3}, u8{3}, static_cast<TestType>(0));
+        REQUIRE(val == static_cast<TestType>(0xF7));
+    }
+
+    SECTION("setting value 0 clears the range") {
+        TestType val = static_cast<TestType>(0xFF);
+        set_bits(val, u8{0}, u8{7}, static_cast<TestType>(0));
+        REQUIRE(val == TestType{0});
+    }
+}
+
+TEST_CASE("set_bits with different value type (U != T)", "[set_bits]") {
+    u32 val = 0;
+    constexpr u8 small_val = 0x3F;
+    set_bits(val, u8{8}, u8{13}, small_val);
+    REQUIRE(val == (u32{0x3F} << 8));
+}
+
+TEST_CASE("set_bits preserves surrounding bits in 32-bit", "[set_bits]") {
+    u32 val = 0xDEADBEEF;
+    set_bits(val, u8{8}, u8{15}, u32{0x42});
+    REQUIRE(val == 0xDEAD42EF);
+}
+
+// ---------------------------------------------------------------------------
+// get_bits
+// ---------------------------------------------------------------------------
+
+TEMPLATE_TEST_CASE("get_bits extracts the specified range", "[get_bits]",
+                   u8, u16, u32, u64)
+{
+    SECTION("get lower nibble") {
+        TestType val = static_cast<TestType>(0xAB);
+        auto result = get_bits(val, u8{0}, u8{3});
+        REQUIRE(result == TestType{0xB});
+    }
+
+    SECTION("get upper nibble") {
+        TestType val = static_cast<TestType>(0xAB);
+        auto result = get_bits(val, u8{4}, u8{7});
+        REQUIRE(result == TestType{0xA});
+    }
+
+    SECTION("get single bit that is set") {
+        TestType val = static_cast<TestType>(0x08);
+        auto result = get_bits(val, u8{3}, u8{3});
+        REQUIRE(result == TestType{1});
+    }
+
+    SECTION("get single bit that is clear") {
+        TestType val = static_cast<TestType>(0xF7);
+        auto result = get_bits(val, u8{3}, u8{3});
+        REQUIRE(result == TestType{0});
+    }
+
+    SECTION("get all bits") {
+        TestType val = static_cast<TestType>(~TestType{0});
+        constexpr u8 last = sizeof(TestType) * 8 - 1;
+        auto result = get_bits(val, u8{0}, last);
+        REQUIRE(result == val);
+    }
+
+    SECTION("get from zero returns zero") {
+        TestType val = TestType{0};
+        auto result = get_bits(val, u8{0}, u8{7});
+        REQUIRE(result == TestType{0});
+    }
+}
+
+TEST_CASE("get_bits 32-bit specific extractions", "[get_bits]") {
+    constexpr u32 val = 0xDEADBEEF;
+
+    REQUIRE(get_bits(val, u8{0},  u8{7})  == 0xEF);
+    REQUIRE(get_bits(val, u8{8},  u8{15}) == 0xBE);
+    REQUIRE(get_bits(val, u8{16}, u8{23}) == 0xAD);
+    REQUIRE(get_bits(val, u8{24}, u8{31}) == 0xDE);
+}
+
+// ---------------------------------------------------------------------------
+// Round-trip: set then get
+// ---------------------------------------------------------------------------
+
+TEST_CASE("set_bits then get_bits round-trips correctly", "[round-trip]") {
+    u32 reg = 0;
+
+    set_bits(reg, u8{4}, u8{11}, u32{0xAB});
+    REQUIRE(get_bits(reg, u8{4}, u8{11}) == 0xAB);
+
+    REQUIRE(get_bits(reg, u8{0},  u8{3})  == 0x0);
+    REQUIRE(get_bits(reg, u8{12}, u8{31}) == 0x0);
+}
+
+TEST_CASE("multiple set_bits on different ranges", "[round-trip]") {
+    u32 reg = 0;
+
+    set_bits(reg, u8{0},  u8{7},  u32{0x01});
+    set_bits(reg, u8{8},  u8{15}, u32{0x02});
+    set_bits(reg, u8{16}, u8{23}, u32{0x03});
+    set_bits(reg, u8{24}, u8{31}, u32{0x04});
+
+    REQUIRE(reg == 0x04030201);
+}
+
+TEST_CASE("64-bit round-trip", "[round-trip]") {
+    u64 reg = 0;
+    set_bits(reg, u8{32}, u8{63}, u64{0xCAFEBABE});
+    REQUIRE(get_bits(reg, u8{32}, u8{63}) == u64{0xCAFEBABE});
+    REQUIRE(get_bits(reg, u8{0},  u8{31}) == u64{0});
+}