PhysicsLayoutSimulation.cpp

#include "PhysicsLayoutSimulation.h"
#include <cstdio>
#include <cmath>
#include <cstring>
#include <set>
#include <fstream>
#include <sstream>

#ifndef GL_COMPUTE_SHADER
#define GL_COMPUTE_SHADER 0x91B9
#endif
#ifndef GL_SHADER_STORAGE_BUFFER
#define GL_SHADER_STORAGE_BUFFER 0x90D2
#endif
#ifndef GL_SHADER_STORAGE_BARRIER_BIT
#define GL_SHADER_STORAGE_BARRIER_BIT 0x00002000
#endif

PhysicsLayoutSimulation::~PhysicsLayoutSimulation() {
    if (particleBuffer_) glDeleteBuffers(1, &particleBuffer_);
    if (activeBuffer_)   glDeleteBuffers(1, &activeBuffer_);
    if (settleBuffer_)   glDeleteBuffers(1, &settleBuffer_);
    if (wakeBuffer_)     glDeleteBuffers(1, &wakeBuffer_);
    if (densityProgram_) glDeleteProgram(densityProgram_);
    if (forceProgram_)   glDeleteProgram(forceProgram_);
}

GLuint PhysicsLayoutSimulation::CompileComputeFromFile(const char* path) {
    std::ifstream file(path);
    if (!file.is_open()) {
        printf("ERROR: PhysicsLayoutSimulation: cannot open shader '%s'\n", path);
        return 0;
    }
    std::stringstream ss;
    ss << file.rdbuf();
    std::string src = ss.str();
    const char* csrc = src.c_str();

    GLuint shader = glCreateShader(GL_COMPUTE_SHADER);
    glShaderSource(shader, 1, &csrc, nullptr);
    glCompileShader(shader);
    GLint ok = 0;
    glGetShaderiv(shader, GL_COMPILE_STATUS, &ok);
    if (!ok) {
        char log[1024];
        glGetShaderInfoLog(shader, sizeof(log), nullptr, log);
        printf("ERROR: PhysicsLayoutSimulation: compile failed '%s':\n%s\n", path, log);
        glDeleteShader(shader);
        return 0;
    }

    GLuint program = glCreateProgram();
    glAttachShader(program, shader);
    glLinkProgram(program);
    glGetProgramiv(program, GL_LINK_STATUS, &ok);
    glDeleteShader(shader);
    if (!ok) {
        char log[1024];
        glGetProgramInfoLog(program, sizeof(log), nullptr, log);
        printf("ERROR: PhysicsLayoutSimulation: link failed '%s':\n%s\n", path, log);
        glDeleteProgram(program);
        return 0;
    }
    return program;
}

bool PhysicsLayoutSimulation::Initialize() {
    if (!glDispatchCompute || !glBindBufferBase || !glMemoryBarrier) {
        printf("ERROR: PhysicsLayoutSimulation requires GL 4.3+ compute functions\n");
        return false;
    }

    densityProgram_ = CompileComputeFromFile("shaders/sph_density.glsl");
    if (!densityProgram_) return false;
    forceProgram_ = CompileComputeFromFile("shaders/sph_force.glsl");
    if (!forceProgram_) return false;

    glGenBuffers(1, &particleBuffer_);
    glGenBuffers(1, &activeBuffer_);
    glGenBuffers(1, &settleBuffer_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, settleBuffer_);
    glBufferData(GL_SHADER_STORAGE_BUFFER, sizeof(uint32_t), nullptr, GL_DYNAMIC_DRAW);
    glGenBuffers(1, &wakeBuffer_);  // sized lazily in Step() to match particleCount_

    isSupported_ = true;
    printf("PhysicsLayoutSimulation: initialized\n");
    return true;
}

int PhysicsLayoutSimulation::GetParticleSlot(int nodeId) const {
    auto it = slotById_.find(nodeId);
    return (it != slotById_.end()) ? it->second : -1;
}

void PhysicsLayoutSimulation::SeedFromLayout(const std::map<int, ImVec2>& nodePositions,
                                             const std::map<int, ImVec2>& nodeSizes) {
    if (!isSupported_) return;

    slotById_.clear();
    idBySlot_.clear();
    idBySlot_.reserve(nodePositions.size());
    fixedHomes_.clear();

    std::vector<Particle> particles;
    particles.reserve(nodePositions.size());

    for (const auto& [id, pos] : nodePositions) {
        // Skip dummy nodes (negative ids): routing waypoints, not physical particles.
        if (id < 0) continue;

        int slot = (int)particles.size();
        slotById_[id] = slot;
        idBySlot_.push_back(id);
        fixedHomes_[id] = pos;  // original Sugiyama slot = the fixed home, captured once

        ImVec2 size(150.0f, 50.0f);
        auto sit = nodeSizes.find(id);
        if (sit != nodeSizes.end()) size = sit->second;

        Particle p{};
        p.pos      = pos;
        p.vel      = ImVec2(0.0f, 0.0f);
        p.home     = pos;
        p.halfSize = ImVec2(size.x * 0.5f, size.y * 0.5f);
        p.density   = 0.0f;
        p.pressure  = 0.0f;
        p.flags     = PARTICLE_ACTIVE;
        p.homeScale = 1.0f;   // baseline; BeginTransition overrides per role
        particles.push_back(p);
    }

    particleCount_ = (int)particles.size();

    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    size_t needed = particles.size() * sizeof(Particle);
    if (particles.size() > particleCapacity_) {
        particleCapacity_ = particles.size();
        glBufferData(GL_SHADER_STORAGE_BUFFER, needed, particles.data(), GL_DYNAMIC_DRAW);
    } else {
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, needed, particles.data());
    }

    // Fresh seed sits exactly on home (zero displacement) -> settled until something kicks it.
    settled_ = true;
    printf("PhysicsLayoutSimulation: seeded %d particles\n", particleCount_);
}

void PhysicsLayoutSimulation::NudgeAll(float strength) {
    if (!isSupported_ || particleCount_ == 0) return;

    std::vector<Particle> particles(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), particles.data());

    // Deterministic per-slot pseudo-random impulse (no Math.random needed for reproducibility).
    for (int i = 0; i < particleCount_; i++) {
        uint32_t h = (uint32_t)i * 2654435761u;   // Knuth multiplicative hash
        float ang = (float)(h & 0xFFFF) / 65535.0f * 6.2831853f;
        particles[i].vel.x += strength * cosf(ang);
        particles[i].vel.y += strength * sinf(ang);
    }

    glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                    particleCount_ * sizeof(Particle), particles.data());

    // Debug case: everyone participates.
    std::vector<uint32_t> all(particleCount_);
    for (int i = 0; i < particleCount_; i++) all[i] = (uint32_t)i;
    UploadActive(all);

    Kick();
    printf("PhysicsLayoutSimulation: nudged %d particles (strength=%.1f)\n", particleCount_, strength);
}

void PhysicsLayoutSimulation::SetActiveSubset(const std::vector<int>& nodeIds) {
    if (!isSupported_) return;

    std::vector<uint32_t> slots;
    slots.reserve(nodeIds.size());
    for (int id : nodeIds) {
        int slot = GetParticleSlot(id);
        if (slot >= 0) slots.push_back((uint32_t)slot);
    }
    UploadActive(slots);
}

void PhysicsLayoutSimulation::BeginTransition(const std::map<int, ImVec2>& homes,
                                              const std::map<int, ImVec2>& sizes,
                                              ImVec2 spawnPoint,
                                              const std::vector<int>& activeSubset) {
    if (!isSupported_) return;

    // Snapshot the CURRENT particle state so surviving nodes keep their live position/velocity
    // (animate from where they are, not a snap). Newcomers won't be found here.
    std::vector<Particle> old(particleCount_);
    if (particleCount_ > 0) {
        glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
        glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                           particleCount_ * sizeof(Particle), old.data());
    }
    std::map<int, int> oldSlotById = slotById_;  // id -> old slot (before we rebuild)

    // Rebuild the dense slot map for the NEW node set (== keys of `homes`, minus dummies).
    slotById_.clear();
    idBySlot_.clear();
    idBySlot_.reserve(homes.size());

    std::vector<Particle> particles;
    particles.reserve(homes.size());

    for (const auto& [id, home] : homes) {
        if (id < 0) continue;  // dummy waypoint, not a physical particle

        int slot = (int)particles.size();
        slotById_[id] = slot;
        idBySlot_.push_back(id);

        ImVec2 size(150.0f, 50.0f);
        auto sit = sizes.find(id);
        if (sit != sizes.end()) size = sit->second;

        Particle p{};
        p.halfSize = ImVec2(size.x * 0.5f, size.y * 0.5f);
        p.density  = 0.0f;
        p.pressure = 0.0f;
        p.flags    = PARTICLE_ACTIVE;

        auto oit = oldSlotById.find(id);
        if (oit != oldSlotById.end() && oit->second < (int)old.size()) {
            // Surviving node: keep current position + velocity AND its ORIGINAL fixed home.
            // Neighbors are NOT retargeted - they're displaced only by emergent SPH pressure and
            // spring back to their fixed slot. This is what keeps nested expand/collapse correct.
            p.pos       = old[oit->second].pos;
            p.vel       = old[oit->second].vel;
            p.home      = old[oit->second].home;
            p.homeScale = neighborHomeScale_;  // loose: yield to pressure
            // flags already = PARTICLE_ACTIVE (no NO_COLLIDE): survivors collide as neighbors.
        } else {
            // Newcomer (freshly-expanded child): its home is the caller-provided mini-Sugiyama
            // slot, and it spawns AT the parent's collapsed slot with a small deterministic
            // jitter so particles don't stack exactly (the SPH pressure gradient needs a
            // direction to push them apart toward their homes).
            p.home      = home;
            p.homeScale = childHomeScale_;  // firm: converge decisively to the subgraph slot
            p.flags    |= PARTICLE_NO_COLLIDE;  // transient: spawns stacked at a point, opts out of collision
            fixedHomes_[id] = home;         // child's mini-Sugiyama slot is its fixed home
            uint32_t h = (uint32_t)(slot + 1) * 2654435761u;   // Knuth multiplicative hash
            float ang = (float)(h & 0xFFFF) / 65535.0f * 6.2831853f;
            const float jitter = 2.0f;  // px; tiny, just to break the symmetry
            p.pos = ImVec2(spawnPoint.x + cosf(ang) * jitter,
                           spawnPoint.y + sinf(ang) * jitter);
            p.vel = ImVec2(0.0f, 0.0f);
        }

        particles.push_back(p);
    }

    particleCount_ = (int)particles.size();

    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    size_t needed = particles.size() * sizeof(Particle);
    if (particles.size() > particleCapacity_) {
        particleCapacity_ = particles.size();
        glBufferData(GL_SHADER_STORAGE_BUFFER, needed, particles.data(), GL_DYNAMIC_DRAW);
    } else {
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, needed, particles.data());
    }

    SetActiveSubset(activeSubset);
    Kick();
    printf("PhysicsLayoutSimulation: transition begun (%d particles, %d active, spawn=%.0f,%.0f)\n",
           particleCount_, activeCount_, spawnPoint.x, spawnPoint.y);
}

void PhysicsLayoutSimulation::BeginCollapse(const std::vector<int>& collapsedIds,
                                            ImVec2 centroid,
                                            const std::vector<int>& activeSubset) {
    if (!isSupported_ || particleCount_ == 0) return;

    std::vector<Particle> particles(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), particles.data());

    std::set<int> collapsing(collapsedIds.begin(), collapsedIds.end());

    for (int slot = 0; slot < particleCount_; slot++) {
        int id = (slot < (int)idBySlot_.size()) ? idBySlot_[slot] : -1;
        if (collapsing.count(id)) {
            // Child being collapsed: retarget its home so its CENTER converges on the centroid and
            // pull firmly, so it rushes inward and piles up at the point. `home` is a top-left, so
            // subtract halfSize - otherwise the child's top-left lands on the centroid and the pile
            // sits half a node down-and-right of where the parent (drawn centered) reappears.
            particles[slot].home      = ImVec2(centroid.x - particles[slot].halfSize.x,
                                               centroid.y - particles[slot].halfSize.y);
            particles[slot].homeScale = collapseHomeScale_;
            particles[slot].flags    |= PARTICLE_NO_COLLIDE;  // piling at a point: opt out of collision
            particles[slot].flags    |= PARTICLE_FADING;      // fade out as it nears the centroid
        } else {
            // Everyone else (neighbors): re-enable a gentle home spring so they flow BACK toward
            // their fixed slots as the children's pressure fades, filling the vacated space.
            // (Their home is unchanged - still the fixed slot stored since seed/expand.)
            particles[slot].homeScale = neighborReturnScale_;
            particles[slot].flags    &= ~PARTICLE_NO_COLLIDE;  // neighbors DO collide (no overlap)
        }
    }

    glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                    particleCount_ * sizeof(Particle), particles.data());

    SetActiveSubset(activeSubset);
    Kick();
    printf("PhysicsLayoutSimulation: collapse begun (%zu collapsing, %d active, centroid=%.0f,%.0f)\n",
           collapsedIds.size(), activeCount_, centroid.x, centroid.y);
}

void PhysicsLayoutSimulation::RemoveParticles(const std::vector<int>& nodeIds) {
    if (!isSupported_ || particleCount_ == 0) return;

    std::set<int> remove(nodeIds.begin(), nodeIds.end());

    std::vector<Particle> old(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), old.data());

    // Compact: keep every particle whose id is not being removed, rebuilding the dense slot map.
    std::vector<Particle> kept;
    kept.reserve(old.size());
    std::map<int, int> newSlotById;
    std::vector<int>   newIdBySlot;
    for (int slot = 0; slot < particleCount_; slot++) {
        int id = (slot < (int)idBySlot_.size()) ? idBySlot_[slot] : -1;
        if (remove.count(id)) continue;
        newSlotById[id] = (int)kept.size();
        newIdBySlot.push_back(id);
        kept.push_back(old[slot]);
    }

    slotById_   = std::move(newSlotById);
    idBySlot_   = std::move(newIdBySlot);
    particleCount_ = (int)kept.size();

    // Removed children no longer have a fixed home (recomputed fresh if re-expanded).
    for (int id : remove) fixedHomes_.erase(id);

    if (particleCount_ > 0) {
        glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                        particleCount_ * sizeof(Particle), kept.data());
    }
}

void PhysicsLayoutSimulation::AddParticleAtHome(int nodeId, ImVec2 home, ImVec2 size) {
    if (!isSupported_ || nodeId < 0) return;
    if (slotById_.count(nodeId)) {           // already present -> just refresh its home
        fixedHomes_[nodeId] = home;
        return;
    }

    Particle p{};
    p.pos       = home;
    p.vel       = ImVec2(0.0f, 0.0f);
    p.home      = home;
    p.halfSize  = ImVec2(size.x * 0.5f, size.y * 0.5f);
    p.density   = 0.0f;
    p.pressure  = 0.0f;
    p.flags     = PARTICLE_ACTIVE;
    p.homeScale = 1.0f;

    int slot = particleCount_;
    slotById_[nodeId] = slot;
    idBySlot_.push_back(nodeId);
    fixedHomes_[nodeId] = home;
    particleCount_ = slot + 1;

    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    if ((size_t)particleCount_ > particleCapacity_) {
        // Grow: reupload the whole buffer (rare; only when exceeding prior capacity).
        std::vector<Particle> all(particleCount_ - 1);
        if (particleCount_ - 1 > 0) {
            glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                               (particleCount_ - 1) * sizeof(Particle), all.data());
        }
        all.push_back(p);
        particleCapacity_ = particleCount_;
        glBufferData(GL_SHADER_STORAGE_BUFFER, particleCount_ * sizeof(Particle),
                     all.data(), GL_DYNAMIC_DRAW);
    } else {
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, slot * sizeof(Particle), sizeof(Particle), &p);
    }
}

ImVec2 PhysicsLayoutSimulation::GetFixedHome(int nodeId) const {
    auto it = fixedHomes_.find(nodeId);
    return (it != fixedHomes_.end()) ? it->second : ImVec2(0.0f, 0.0f);
}

void PhysicsLayoutSimulation::SetHome(int nodeId, ImVec2 home) {
    if (!isSupported_) return;
    fixedHomes_[nodeId] = home;

    // Also move the live particle's pos + home so the spring targets the new spot (the node was
    // just dragged there). Single-particle SSBO write; cheap.
    int slot = GetParticleSlot(nodeId);
    if (slot < 0) return;
    Particle p{};
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, slot * sizeof(Particle), sizeof(Particle), &p);
    p.home = home;
    p.pos  = home;
    p.vel  = ImVec2(0.0f, 0.0f);
    glBufferSubData(GL_SHADER_STORAGE_BUFFER, slot * sizeof(Particle), sizeof(Particle), &p);
}

void PhysicsLayoutSimulation::SnapSettledToHome(float threshold) {
    if (!isSupported_ || particleCount_ == 0) return;

    std::vector<Particle> particles(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), particles.data());

    float t2 = threshold * threshold;
    int snapped = 0;
    for (int slot = 0; slot < particleCount_; slot++) {
        int id = (slot < (int)idBySlot_.size()) ? idBySlot_[slot] : -1;
        auto it = fixedHomes_.find(id);
        if (it == fixedHomes_.end()) continue;

        ImVec2 home = it->second;
        float dx = particles[slot].pos.x - home.x;
        float dy = particles[slot].pos.y - home.y;
        if (dx * dx + dy * dy <= t2) {
            // "Free" node near its slot: land it exactly on home. Nodes still displaced by an
            // expanded subgraph are beyond the threshold and left alone (stay pushed out).
            particles[slot].pos = home;
            particles[slot].vel = ImVec2(0.0f, 0.0f);
            snapped++;
        }
    }

    glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                    particleCount_ * sizeof(Particle), particles.data());
    printf("PhysicsLayoutSimulation: snapped %d/%d particles to home (threshold=%.0f)\n",
           snapped, particleCount_, threshold);
}

void PhysicsLayoutSimulation::SnapAllToHome() {
    if (!isSupported_ || particleCount_ == 0) return;

    std::vector<Particle> particles(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), particles.data());

    for (int slot = 0; slot < particleCount_; slot++) {
        int id = (slot < (int)idBySlot_.size()) ? idBySlot_[slot] : -1;
        auto it = fixedHomes_.find(id);
        if (it == fixedHomes_.end()) continue;   // no known home -> leave as-is
        particles[slot].pos = it->second;
        particles[slot].vel = ImVec2(0.0f, 0.0f);
    }

    glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                    particleCount_ * sizeof(Particle), particles.data());
    printf("PhysicsLayoutSimulation: snapped ALL %d particles to fixed home (no subgraphs expanded)\n",
           particleCount_);
}

void PhysicsLayoutSimulation::ReadBackPositions(std::map<int, ImVec2>& outPositions) const {
    if (!isSupported_ || particleCount_ == 0) return;

    std::vector<Particle> particles(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0,
                       particleCount_ * sizeof(Particle), particles.data());

    for (int slot = 0; slot < particleCount_; slot++) {
        if (slot < (int)idBySlot_.size()) {
            outPositions[idBySlot_[slot]] = particles[slot].pos;
        }
    }
}

void PhysicsLayoutSimulation::UploadActive(const std::vector<uint32_t>& slots) {
    activeCount_ = (int)slots.size();

    // Track which node ids are active (for wake-on-contact dedup).
    activeIds_.clear();
    for (uint32_t s : slots) {
        if ((int)s < (int)idBySlot_.size()) activeIds_.insert(idBySlot_[s]);
    }

    if (activeCount_ == 0) return;

    glBindBuffer(GL_SHADER_STORAGE_BUFFER, activeBuffer_);
    size_t needed = slots.size() * sizeof(uint32_t);
    if (slots.size() > activeCapacity_) {
        activeCapacity_ = slots.size();
        glBufferData(GL_SHADER_STORAGE_BUFFER, needed, slots.data(), GL_DYNAMIC_DRAW);
    } else {
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, needed, slots.data());
    }
}

void PhysicsLayoutSimulation::Step(float dt) {
    if (!isSupported_ || settled_ || activeCount_ == 0) return;

    GLuint groups = ((GLuint)activeCount_ + 255) / 256;

    // Passes read/write these SSBOs (particles @0, active list @1, settle scratch @2, wake @3).
    glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, particleBuffer_);
    glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 1, activeBuffer_);
    glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 2, settleBuffer_);
    glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 3, wakeBuffer_);

    // Reset the max-displacement accumulator to 0 before this step's force pass writes it.
    uint32_t zero = 0u;
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, settleBuffer_);
    glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, sizeof(uint32_t), &zero);

    // Ensure the wake buffer holds one uint per particle, cleared to 0 each step. The collision
    // loop sets wake[j]=1 when an active particle overlaps particle j.
    std::vector<uint32_t> wakeClear(particleCount_, 0u);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, wakeBuffer_);
    if ((size_t)particleCount_ > wakeCapacity_) {
        wakeCapacity_ = particleCount_;
        glBufferData(GL_SHADER_STORAGE_BUFFER, particleCount_ * sizeof(uint32_t),
                     wakeClear.data(), GL_DYNAMIC_DRAW);
    } else {
        glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, particleCount_ * sizeof(uint32_t),
                        wakeClear.data());
    }

    // Pass 1: density + pressure. Must complete globally before forces read densities.
    glUseProgram(densityProgram_);
    glUniform1ui(glGetUniformLocation(densityProgram_, "numActive"),       (GLuint)activeCount_);
    glUniform1f (glGetUniformLocation(densityProgram_, "smoothingRadius"), smoothingRadius_);
    glUniform1f (glGetUniformLocation(densityProgram_, "particleMass"),    particleMass_);
    glUniform1f (glGetUniformLocation(densityProgram_, "gasConstant"),     gasConstant_);
    glUniform1f (glGetUniformLocation(densityProgram_, "restDensity"),     restDensity_);
    glDispatchCompute(groups, 1, 1);
    glMemoryBarrier(GL_SHADER_STORAGE_BARRIER_BIT);

    // Pass 2: pressure + viscosity + home spring, then integrate.
    glUseProgram(forceProgram_);
    glUniform1ui(glGetUniformLocation(forceProgram_, "numActive"),       (GLuint)activeCount_);
    glUniform1ui(glGetUniformLocation(forceProgram_, "numParticles"),    (GLuint)particleCount_);
    glUniform1f (glGetUniformLocation(forceProgram_, "dt"),              dt);
    glUniform1f (glGetUniformLocation(forceProgram_, "smoothingRadius"), smoothingRadius_);
    glUniform1f (glGetUniformLocation(forceProgram_, "particleMass"),    particleMass_);
    glUniform1f (glGetUniformLocation(forceProgram_, "viscosity"),       viscosity_);
    glUniform2f (glGetUniformLocation(forceProgram_, "homeStiffness"),   homeStiffnessX_, homeStiffnessY_);
    glUniform1f (glGetUniformLocation(forceProgram_, "damping"),         damping_);
    glUniform1f (glGetUniformLocation(forceProgram_, "maxSpeed"),        maxSpeed_);
    glUniform1f (glGetUniformLocation(forceProgram_, "collisionStrength"), collisionStrength_);
    glUniform1f (glGetUniformLocation(forceProgram_, "maxHomeDistance"), maxHomeDistance_);
    glDispatchCompute(groups, 1, 1);
    glMemoryBarrier(GL_SHADER_STORAGE_BARRIER_BIT);

    // Settle detection: read back this step's max per-step displacement (one uint) and freeze
    // once the sim has been quiet (max move < settleEpsilon_) for enough consecutive frames.
    // A hard frame cap guarantees termination even if it never fully quiesces.
    framesSinceKick_++;
    uint32_t maxBits = 0u;
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, settleBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, sizeof(uint32_t), &maxBits);
    float maxDisp;
    memcpy(&maxDisp, &maxBits, sizeof(float));

    if (maxDisp < settleEpsilon_) {
        if (++quietFrames_ >= settleQuietNeeded_) settled_ = true;
    } else {
        quietFrames_ = 0;
    }
    if (framesSinceKick_ >= settleMaxFrames_) {
        settled_ = true;  // safety cap
        printf("PhysicsLayoutSimulation: settle hit frame cap (%d) at maxDisp=%.2f\n",
               settleMaxFrames_, maxDisp);
    }

    // Wake-on-contact: read back the wake flags. Any particle flagged (overlapped by an active
    // node) that isn't already active gets added to the active set, so it can move away next step.
    // This propagates a shove outward one ring per frame. Keeps the sim awake while it's growing.
    std::vector<uint32_t> wakeFlags(particleCount_);
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, wakeBuffer_);
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, particleCount_ * sizeof(uint32_t), wakeFlags.data());

    std::vector<int> newlyWoken;
    for (int slot = 0; slot < particleCount_; slot++) {
        if (wakeFlags[slot] == 0u) continue;
        int id = (slot < (int)idBySlot_.size()) ? idBySlot_[slot] : -1;
        if (id < 0) continue;
        if (activeIds_.count(id)) continue;  // already active
        newlyWoken.push_back(id);
    }

    if (!newlyWoken.empty()) {
        // Rebuild the active set = current active ids + newly woken. Re-upload as slots.
        for (int id : newlyWoken) activeIds_.insert(id);
        std::vector<uint32_t> slots;
        slots.reserve(activeIds_.size());
        for (int id : activeIds_) {
            int s = GetParticleSlot(id);
            if (s >= 0) slots.push_back((uint32_t)s);
        }
        UploadActive(slots);      // refreshes activeCount_ + activeIds_
        settled_ = false;         // set grew -> keep simulating
        quietFrames_ = 0;
    }
}

ImVec2 PhysicsLayoutSimulation::GetParticlePosition(int nodeId) const {
    int slot = GetParticleSlot(nodeId);
    if (slot < 0) return ImVec2(0.0f, 0.0f);

    Particle p{};
    glBindBuffer(GL_SHADER_STORAGE_BUFFER, particleBuffer_);
    // glGetBufferSubData is available via GLFunctions.
    glGetBufferSubData(GL_SHADER_STORAGE_BUFFER, slot * sizeof(Particle), sizeof(Particle), &p);
    return p.pos;
}