flowfield.cpp.hpp

Go to the documentation of this file.

 
#include "fl/fx/2d/flowfield.h"
 
#include "fl/fx/2d/animartrix_detail/perlin_s16x16.h"
#include "fl/gfx/tile2x2.h"
#include "fl/math/math.h"
#include "fl/stl/compiler_control.h"
 
namespace fl {
 
// ===========================================================================
//  FlowField base class
// ===========================================================================
 
FlowField::FlowField(const XYMap &xyMap, const Params &params)
    : Fx2d(xyMap), mParams(params),
      mNoiseBias(xyMap.getWidth(), xyMap.getHeight(), 0.01f, 0.5f) {}
 
void FlowField::draw(DrawContext context) {
    u32 t_ms = mTimeWarp.update(context.now);
    u32 dt_ms = t_ms - mLastWarpedMs;
    mLastWarpedMs = t_ms;
    // Cap dt to prevent huge jumps when effect was inactive (mode switch).
    if (dt_ms > 500) {
        dt_ms = 0;
    }
    mNoiseBias.update(dt_ms * 0.001f);
    drawImpl(context, dt_ms, t_ms);
}
 
void FlowField::noisePunch(float amplitude, BumpShape shape) {
    float cx = getWidth() * 0.5f;
    float cy = getHeight() * 0.5f;
    float wx = getWidth() * 0.4f;
    float wy = getHeight() * 0.4f;
    mNoiseBias.triggerX(cx, wx, amplitude, shape);
    mNoiseBias.triggerY(cy, wy, amplitude, shape);
}
 
void FlowField::noisePunchX(float center, float width, float amplitude,
                             BumpShape shape) {
    mNoiseBias.triggerX(center, width, amplitude, shape);
}
 
void FlowField::noisePunchY(float center, float width, float amplitude,
                             BumpShape shape) {
    mNoiseBias.triggerY(center, width, amplitude, shape);
}
 
// ===========================================================================
//  FlowFieldFloat
// ===========================================================================
 
// ---------------------------------------------------------------------------
//  Perlin2D
// ---------------------------------------------------------------------------
 
void FlowFieldFloat::Perlin2D::init(u32 seed) {
    u8 p[256];
    for (int i = 0; i < 256; i++)
        p[i] = (u8)i;
    u32 s = seed;
    for (int i = 255; i > 0; i--) {
        s = s * 1664525u + 1013904223u;
        int j = (int)((s >> 16) % (u32)(i + 1));
        u8 tmp = p[i];
        p[i] = p[j];
        p[j] = tmp;
    }
    for (int i = 0; i < 256; i++) {
        perm[i] = p[i];
        perm[i + 256] = p[i];
    }
}
 
float FlowFieldFloat::Perlin2D::noise(float x, float y) const {
    int xi = ((int)floorf(x)) & 255;
    int yi = ((int)floorf(y)) & 255;
    float xf = x - floorf(x);
    float yf = y - floorf(y);
    float u = fade(xf);
    float v = fade(yf);
    int aa = perm[perm[xi] + yi];
    int ab = perm[perm[xi] + yi + 1];
    int ba = perm[perm[xi + 1] + yi];
    int bb = perm[perm[xi + 1] + yi + 1];
    float x1 = lerp(grad(aa, xf, yf), grad(ba, xf - 1.0f, yf), u);
    float x2 = lerp(grad(ab, xf, yf - 1.0f),
                     grad(bb, xf - 1.0f, yf - 1.0f), u);
    return lerp(x1, x2, v);
}
 
float FlowFieldFloat::Perlin2D::grad(int h, float x, float y) {
    switch (h & 7) {
    case 0:
        return x + y;
    case 1:
        return -x + y;
    case 2:
        return x - y;
    case 3:
        return -x - y;
    case 4:
        return x;
    case 5:
        return -x;
    case 6:
        return y;
    default:
        return -y;
    }
}
 
// ---------------------------------------------------------------------------
 
FlowFieldFloat::FlowFieldFloat(const XYMap &xyMap, const Params &params)
    : FlowField(xyMap, params) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    int n = w * h;
 
    mR.resize(n, 0.0f);
    mG.resize(n, 0.0f);
    mB.resize(n, 0.0f);
    mTR.resize(n, 0.0f);
    mTG.resize(n, 0.0f);
    mTB.resize(n, 0.0f);
    mXProf.resize(w, 0.0f);
    mYProf.resize(h, 0.0f);
 
    mNoiseGenX.init(42);
    mNoiseGenY.init(1337);
}
 
float FlowFieldFloat::fmodPos(float x, float m) {
    float r = fmodf(x, m);
    return r < 0.0f ? r + m : r;
}
 
float FlowFieldFloat::clampf(float v, float lo, float hi) {
    return (v < lo) ? lo : (v > hi) ? hi : v;
}
 
u8 FlowFieldFloat::f2u8(float v) {
    int i = (int)v;
    if (i < 0)
        return 0;
    if (i > 255)
        return 255;
    return (u8)i;
}
 
CRGB FlowFieldFloat::rainbow(float t, float speed, float phase) {
    float hue = fmodPos(t * speed + phase, 1.0f);
    CHSV hsv((u8)(hue * 255.0f), 255, 255);
    CRGB rgb;
    hsv2rgb_rainbow(hsv, rgb);
    return rgb;
}
 
void FlowFieldFloat::drawDot(float cx, float cy, float diam,
                                  u8 cr, u8 cg, u8 cb) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    float rad = diam * 0.5f;
    int x0 = fl::max(0, (int)floorf(cx - rad - 1.0f));
    int x1 = fl::min(w - 1, (int)ceilf(cx + rad + 1.0f));
    int y0 = fl::max(0, (int)floorf(cy - rad - 1.0f));
    int y1 = fl::min(h - 1, (int)ceilf(cy + rad + 1.0f));
    for (int y = y0; y <= y1; y++) {
        for (int x = x0; x <= x1; x++) {
            float dx = (x + 0.5f) - cx;
            float dy = (y + 0.5f) - cy;
            float dist = sqrtf(dx * dx + dy * dy);
            float cov = clampf(rad + 0.5f - dist, 0.0f, 1.0f);
            if (cov <= 0.0f)
                continue;
            float inv = 1.0f - cov;
            int i = idx(y, x);
            mR[i] = mR[i] * inv + cr * cov;
            mG[i] = mG[i] * inv + cg * cov;
            mB[i] = mB[i] * inv + cb * cov;
        }
    }
}
 
void FlowFieldFloat::drawAALine(float x0, float y0, float x1, float y1,
                                   float t, float colorShift) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    float dx = x1 - x0;
    float dy = y1 - y0;
    int steps = fl::max(1, (int)(fl::max(fabsf(dx), fabsf(dy)) * 3.0f));
    float invSteps = 1.0f / (float)steps;
 
    for (int i = 0; i <= steps; i++) {
        float u = i * invSteps;
        float px = x0 + dx * u;
        float py = y0 + dy * u;
        CRGB c = rainbow(t, colorShift, u);
 
        int ix = (int)floorf(px);
        int iy = (int)floorf(py);
        float fx = px - ix;
        float fy = py - iy;
 
        float weights[4] = {
            (1.0f - fx) * (1.0f - fy),
            fx * (1.0f - fy),
            (1.0f - fx) * fy,
            fx * fy,
        };
        int offX[4] = {0, 1, 0, 1};
        int offY[4] = {0, 0, 1, 1};
 
        for (int j = 0; j < 4; j++) {
            int cx = ix + offX[j];
            int cy = iy + offY[j];
            if (cx < 0 || cx >= w || cy < 0 || cy >= h)
                continue;
            float wt = weights[j];
            if (wt <= 0.0f)
                continue;
            int gi = idx(cy, cx);
            float inv = 1.0f - wt;
            mR[gi] = mR[gi] * inv + c.r * wt;
            mG[gi] = mG[gi] * inv + c.g * wt;
            mB[gi] = mB[gi] * inv + c.b * wt;
        }
    }
}
 
void FlowFieldFloat::emitLissajousLine(float t) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    float cx = (w - 1) * 0.5f;
    float cy = (h - 1) * 0.5f;
    float s = mParams.endpoint_speed;
 
    // Radii scaled from original 32x32 prototype ratios.
    float x1 = cx + cx * 0.742f * sinf(t * s * 1.13f + 0.20f);
    float y1 = cy + cy * 0.677f * sinf(t * s * 1.71f + 1.30f);
    float x2 = cx + cx * 0.774f * sinf(t * s * 1.89f + 2.20f);
    float y2 = cy + cy * 0.710f * sinf(t * s * 1.37f + 0.70f);
 
    drawAALine(x1, y1, x2, y2, t, mParams.color_shift);
 
    CRGB endA = rainbow(t, mParams.color_shift, 0.0f);
    CRGB endB = rainbow(t, mParams.color_shift, 0.5f);
    float discDiam = 1.7f;
    drawDot(x1, y1, discDiam, endA.r, endA.g, endA.b);
    drawDot(x2, y2, discDiam, endB.r, endB.g, endB.b);
}
 
void FlowFieldFloat::emitOrbitalDots(float t) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    int minDim = fl::min(w, h);
    int n = mParams.dot_count;
    float fn = (float)n;
    float ocx = w * 0.5f - 0.5f;
    float ocy = h * 0.5f - 0.5f;
    float orad = minDim * 0.35f;
    float base = t * 3.0f;
    float dotDiam = 1.5f;
    for (int i = 0; i < n; i++) {
        float a = base + i * (2.0f * FL_PI / fn);
        float cx = ocx + cosf(a) * orad;
        float cy = ocy + sinf(a) * orad;
        CRGB c = rainbow(t, mParams.color_shift, (float)i / fn);
        drawDot(cx, cy, dotDiam, c.r, c.g, c.b);
    }
}
 
void FlowFieldFloat::flowPrepare(float t) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    const float kBaseFreq = 0.23f;
 
    for (int i = 0; i < w; i++) {
        float v = mNoiseGenX.noise(
            i * kBaseFreq * mParams.noise_freq_x + t * mParams.flow_speed_x,
            0.0f);
        mXProf[i] = clampf(v * mParams.flow_amp_x, -1.0f, 1.0f);
    }
 
    if (mParams.reverse_x_profile) {
        for (int i = 0; i < w / 2; i++) {
            float tmp = mXProf[i];
            mXProf[i] = mXProf[w - 1 - i];
            mXProf[w - 1 - i] = tmp;
        }
    }
 
    for (int i = 0; i < h; i++) {
        float v = mNoiseGenY.noise(
            i * kBaseFreq * mParams.noise_freq_y + t * mParams.flow_speed_y,
            0.0f);
        mYProf[i] = clampf(v * mParams.flow_amp_y, -1.0f, 1.0f);
    }
 
    // Apply noise bias (attack/decay bumps from noisePunch triggers).
    for (int i = 0; i < w; i++) {
        mXProf[i] = clampf(mXProf[i] + mNoiseBias.getX(i), -1.0f, 1.0f);
    }
    for (int i = 0; i < h; i++) {
        mYProf[i] = clampf(mYProf[i] + mNoiseBias.getY(i), -1.0f, 1.0f);
    }
}
 
void FlowFieldFloat::flowAdvect(float dt) {
    int w = (int)getWidth();
    int h = (int)getHeight();
    float halfLife = fl::max(mParams.persistence, 0.001f);
    float fade = powf(0.5f, dt / halfLife);
    float shift = mParams.flow_shift;
 
    // Pass 1: horizontal row shift.
    for (int y = 0; y < h; y++) {
        float sh = mYProf[y] * shift;
        for (int x = 0; x < w; x++) {
            float sx = fmodPos((float)x - sh, (float)w);
            int ix0 = (int)floorf(sx) % w;
            int ix1 = (ix0 + 1) % w;
            float f = sx - floorf(sx);
            float inv = 1.0f - f;
            int src0 = idx(y, ix0);
            int src1 = idx(y, ix1);
            int dst = idx(y, x);
            mTR[dst] = mR[src0] * inv + mR[src1] * f;
            mTG[dst] = mG[src0] * inv + mG[src1] * f;
            mTB[dst] = mB[src0] * inv + mB[src1] * f;
        }
    }
 
    // Pass 2: vertical column shift + fade.
    for (int x = 0; x < w; x++) {
        float sh = mXProf[x] * shift;
        for (int y = 0; y < h; y++) {
            float sy = fmodPos((float)y - sh, (float)h);
            int iy0 = (int)floorf(sy) % h;
            int iy1 = (iy0 + 1) % h;
            float f = sy - floorf(sy);
            float inv = 1.0f - f;
            int src0 = idx(iy0, x);
            int src1 = idx(iy1, x);
            int dst = idx(y, x);
            mR[dst] = (mTR[src0] * inv + mTR[src1] * f) * fade;
            mG[dst] = (mTG[src0] * inv + mTG[src1] * f) * fade;
            mB[dst] = (mTB[src0] * inv + mTB[src1] * f) * fade;
        }
    }
}
 
void FlowFieldFloat::drawFlowVectors(fl::span<CRGB> leds) {
    int w = (int)getWidth();
    int h = (int)getHeight();
 
    // Helper: plot a single subpixel point via Tile2x2 through the XYMap.
    auto plotTile = [&](const CRGB &color, float px, float py,
                        bool horiz) {
        int ix = (int)floorf(px);
        int iy = (int)floorf(py);
        float fx = px - (float)ix;
        float fy = py - (float)iy;
        Tile2x2_u8 tile;
        tile.setOrigin((u16)ix, (u16)iy);
        if (horiz) {
            // X profile: subpixel along y only (one column wide)
            tile.at(0, 0) = (u8)((1.0f - fy) * 255.0f);
            tile.at(1, 0) = 0;
            tile.at(0, 1) = (u8)(fy * 255.0f);
            tile.at(1, 1) = 0;
        } else {
            // Y profile: subpixel along x only (one row tall)
            tile.at(0, 0) = (u8)((1.0f - fx) * 255.0f);
            tile.at(1, 0) = (u8)(fx * 255.0f);
            tile.at(0, 1) = 0;
            tile.at(1, 1) = 0;
        }
        tile.draw(color, mXyMap, leds);
    };
 
    // Display amplitude: 0.3 = profile uses ~30% of the display around center.
    constexpr float amp = 0.3f;
 
    // X profile (per-column): plot value as y-position — cyan.
    // Interpolate between consecutive columns to fill gaps.
    CRGB xColor(0, 255, 255);
    float centerY = (float)(h - 1) * 0.5f;
    float prevPy = 0.0f;
    for (int x = 0; x < w; x++) {
        float val = mXProf[x]; // [-1, 1]
        float py = centerY - val * amp * centerY;
        plotTile(xColor, (float)x, py, true);
        if (x > 0) {
            float dy = py - prevPy;
            int gap = (int)fabsf(dy);
            for (int s = 1; s < gap; s++) {
                float t = (float)s / (float)gap;
                float midY = prevPy + dy * t;
                float midX = (float)(x - 1) + t;
                plotTile(xColor, midX, midY, true);
            }
        }
        prevPy = py;
    }
 
    // Y profile (per-row): plot value as x-position — yellow.
    // Interpolate between consecutive rows to fill gaps.
    CRGB yColor(255, 255, 0);
    float centerX = (float)(w - 1) * 0.5f;
    float prevPx = 0.0f;
    for (int y = 0; y < h; y++) {
        float val = mYProf[y]; // [-1, 1]
        float px = centerX + val * amp * centerX;
        plotTile(yColor, px, (float)y, false);
        if (y > 0) {
            float dx = px - prevPx;
            int gap = (int)fabsf(dx);
            for (int s = 1; s < gap; s++) {
                float t = (float)s / (float)gap;
                float midX = prevPx + dx * t;
                float midY = (float)(y - 1) + t;
                plotTile(yColor, midX, midY, false);
            }
        }
        prevPx = px;
    }
}
 
void FlowFieldFloat::drawImpl(DrawContext context, u32 dt_ms, u32 t_ms) {
    float dt = dt_ms * 0.001f;
    float t = t_ms * 0.001f;
 
    flowPrepare(t);
    switch (mParams.emitter_mode) {
    case 0:
        emitLissajousLine(t);
        break;
    case 1:
        emitOrbitalDots(t);
        break;
    case 2:
        emitLissajousLine(t);
        emitOrbitalDots(t);
        break;
    default:
        emitLissajousLine(t);
        break;
    }
    flowAdvect(dt);
 
    int w = (int)getWidth();
    int h = (int)getHeight();
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            int ledIdx = mXyMap.mapToIndex(x, y);
            int i = idx(y, x);
            context.leds.data()[ledIdx].r = f2u8(mR[i]);
            context.leds.data()[ledIdx].g = f2u8(mG[i]);
            context.leds.data()[ledIdx].b = f2u8(mB[i]);
        }
    }
 
    if (mParams.show_flow_vectors) {
        drawFlowVectors(context.leds);
    }
}
 
} // namespace fl
 
// ===========================================================================
//  FlowFieldFP — pure fixed-point (s16x16) implementation
// ===========================================================================
 
FL_OPTIMIZATION_LEVEL_O3_BEGIN
 
namespace fl {
 
// s16x16 constants
static constexpr i32 FP_ONE = s16x16::SCALE;          // 1.0 in Q16.16 = 65536
static constexpr i32 FP_255 = 255 * FP_ONE;           // 255.0
 
// ---------------------------------------------------------------------------
//  Perm table init — Fisher-Yates shuffle (replaces Perlin2D::init)
// ---------------------------------------------------------------------------
 
void FlowFieldFP::initPerm256(u8 *perm, u32 seed) {
    for (int i = 0; i < 256; i++)
        perm[i] = (u8)i;
    u32 s = seed;
    for (int i = 255; i > 0; i--) {
        s = s * 1664525u + 1013904223u;
        int j = (int)((s >> 16) % (u32)(i + 1));
        u8 tmp = perm[i];
        perm[i] = perm[j];
        perm[j] = tmp;
    }
}
 
// ---------------------------------------------------------------------------
//  FlowFieldFP
// ---------------------------------------------------------------------------
 
FlowFieldFP::FlowFieldFP(const XYMap &xyMap, const Params &params)
    : FlowField(xyMap, params) {
    int w = (int)getWidth();
    int h = (int)getHeight();
 
    mState.init(w, h);
    initPerm256(mPermX, 42);
    initPerm256(mPermY, 1337);
    syncParams();
    if (!mState.fade_lut_initialized) {
        perlin_s16x16::init_fade_lut(mState.fade_lut);
        mState.fade_lut_initialized = true;
    }
 
}
 
void FlowFieldFP::syncParams() {
    mColorShift_fp = s16x16(mParams.color_shift);
    mFlowShift_fp = s16x16(mParams.flow_shift);
    mEndpointSpeed_fp = s16x16(mParams.endpoint_speed);
    mPersistence_fp = s16x16(mParams.persistence);
    mNoiseFreqX_fp = s16x16(mParams.noise_freq_x);
    mNoiseFreqY_fp = s16x16(mParams.noise_freq_y);
    mFlowSpeedX_fp = s16x16(mParams.flow_speed_x);
    mFlowSpeedY_fp = s16x16(mParams.flow_speed_y);
    mFlowAmpX_fp = s16x16(mParams.flow_amp_x);
    mFlowAmpY_fp = s16x16(mParams.flow_amp_y);
}
 
i32 FlowFieldFP::clamp_q16(i32 v, i32 lo, i32 hi) {
    return (v < lo) ? lo : (v > hi) ? hi : v;
}
 
u8 FlowFieldFP::q16_to_u8(i32 v) {
    i32 integer = v >> 16;
    if (integer < 0) return 0;
    if (integer > 255) return 255;
    return (u8)integer;
}
 
CRGB FlowFieldFP::rainbow(s16x16 t, s16x16 speed, s16x16 phase) {
    constexpr s16x16 one(1.0f);
    constexpr s16x16 fp_255(255.0f);
    s16x16 hue_raw = t * speed + phase;
    // Positive modulo: hue in [0, 1)
    s16x16 hue = s16x16::mod(hue_raw, one);
    if (hue.raw() < 0)
        hue = hue + one;
    i32 hue_int = s16x16::clamp(hue * fp_255, s16x16(), fp_255).to_int();
    u8 hue_u8 = (u8)hue_int;
    CHSV hsv(hue_u8, 255, 255);
    CRGB rgb;
    hsv2rgb_rainbow(hsv, rgb);
    return rgb;
}
 
// ---------------------------------------------------------------------------
//  Emitters — pure s16x16 fixed-point
// ---------------------------------------------------------------------------
 
void FlowFieldFP::drawDot(s16x16 cx, s16x16 cy, s16x16 diam,
                            u8 cr, u8 cg, u8 cb) {
    int w = mState.width;
    int h = mState.height;
    constexpr s16x16 half(0.5f);
    constexpr s16x16 one(1.0f);
    s16x16 rad = diam * half;
 
    int x0 = fl::max(0, s16x16::floor(cx - rad - one).to_int());
    int x1 = fl::min(w - 1, s16x16::ceil(cx + rad + one).to_int());
    int y0 = fl::max(0, s16x16::floor(cy - rad - one).to_int());
    int y1 = fl::min(h - 1, s16x16::ceil(cy + rad + one).to_int());
 
    s16x16 rad_plus_half = rad + half;
    // Hoist loop-invariant computations
    i32 threshold_sq_raw = (rad_plus_half * rad_plus_half).raw();
    i32 cr_raw = static_cast<i32>(cr) << 16;
    i32 cg_raw = static_cast<i32>(cg) << 16;
    i32 cb_raw = static_cast<i32>(cb) << 16;
    i32 *__restrict__ rp = mState.r.data();
    i32 *__restrict__ gp = mState.g.data();
    i32 *__restrict__ bp = mState.b.data();
 
    for (int y = y0; y <= y1; y++) {
        s16x16 dy = s16x16(y) + half - cy;
        i32 dy_sq_raw = (dy * dy).raw();
        // Early-out: if dy^2 alone exceeds threshold, skip entire row
        if (dy_sq_raw >= threshold_sq_raw) continue;
        int row_base = y * w;
        for (int x = x0; x <= x1; x++) {
            s16x16 dx = s16x16(x) + half - cx;
            i32 dist_sq_raw = dy_sq_raw + (dx * dx).raw();
            if (dist_sq_raw >= threshold_sq_raw)
                continue;
            s16x16 dist = s16x16::sqrt(s16x16::from_raw(dist_sq_raw));
            s16x16 cov = s16x16::clamp(rad_plus_half - dist, s16x16(), one);
            if (cov.raw() <= 0) continue;
            int i = row_base + x;
            // Lerp: old + (new - old) * cov — 1 i64 mul per channel instead of 2
            i32 cov_q16 = cov.raw();
            rp[i] += static_cast<i32>((static_cast<i64>(cr_raw - rp[i]) * cov_q16) >> 16);
            gp[i] += static_cast<i32>((static_cast<i64>(cg_raw - gp[i]) * cov_q16) >> 16);
            bp[i] += static_cast<i32>((static_cast<i64>(cb_raw - bp[i]) * cov_q16) >> 16);
        }
    }
}
 
void FlowFieldFP::drawAALine(s16x16 x0, s16x16 y0, s16x16 x1, s16x16 y1,
                               s16x16 t, s16x16 colorShift) {
    int w = mState.width;
    int h = mState.height;
    s16x16 dx = x1 - x0;
    s16x16 dy = y1 - y0;
    s16x16 max_delta = fl::max(s16x16::abs(dx), s16x16::abs(dy));
    int steps = fl::max(1, (max_delta * s16x16(3)).to_int());
    s16x16 invSteps = s16x16(1) / s16x16(steps);
 
    i32 *__restrict__ rp = mState.r.data();
    i32 *__restrict__ gp = mState.g.data();
    i32 *__restrict__ bp = mState.b.data();
 
    // Precompute step increments (was recomputing from scratch each iteration)
    s16x16 dx_step = dx * invSteps;
    s16x16 dy_step = dy * invSteps;
    s16x16 u = s16x16();
    s16x16 px = x0;
    s16x16 py = y0;
 
    for (int i = 0; i <= steps; i++, u = u + invSteps, px = px + dx_step, py = py + dy_step) {
        CRGB c = rainbow(t, colorShift, u);
 
        int ix = s16x16::floor(px).to_int();
        int iy = s16x16::floor(py).to_int();
        s16x16 fx = s16x16::fract(px);
        s16x16 fy = s16x16::fract(py);
        constexpr s16x16 one(1.0f);
        s16x16 inv_fx = one - fx;
        s16x16 inv_fy = one - fy;
 
        i32 weights[4] = {
            (inv_fx * inv_fy).raw(),
            (fx * inv_fy).raw(),
            (inv_fx * fy).raw(),
            (fx * fy).raw(),
        };
 
        // Pre-shift color values once per step
        i32 cr_raw = static_cast<i32>(c.r) << 16;
        i32 cg_raw = static_cast<i32>(c.g) << 16;
        i32 cb_raw = static_cast<i32>(c.b) << 16;
 
        int offX[4] = {0, 1, 0, 1};
        int offY[4] = {0, 0, 1, 1};
 
        for (int j = 0; j < 4; j++) {
            int cx = ix + offX[j];
            int cy = iy + offY[j];
            if (cx < 0 || cx >= w || cy < 0 || cy >= h)
                continue;
            i32 wt_q16 = weights[j];
            if (wt_q16 <= 0) continue;
            int gi = cy * w + cx;
            // Lerp: old + (new - old) * wt — 1 i64 mul per channel instead of 2
            rp[gi] += static_cast<i32>((static_cast<i64>(cr_raw - rp[gi]) * wt_q16) >> 16);
            gp[gi] += static_cast<i32>((static_cast<i64>(cg_raw - gp[gi]) * wt_q16) >> 16);
            bp[gi] += static_cast<i32>((static_cast<i64>(cb_raw - bp[gi]) * wt_q16) >> 16);
        }
    }
}
 
void FlowFieldFP::emitLissajousLine(s16x16 t) {
    int w = mState.width;
    int h = mState.height;
    constexpr s16x16 half(0.5f);
    s16x16 cx = s16x16(w - 1) * half;
    s16x16 cy = s16x16(h - 1) * half;
 
    // Pre-multiply t * s once (was computed 4 times)
    s16x16 ts = t * mEndpointSpeed_fp;
 
    // Pre-multiply center * radius constants (was computing cx*k and cy*k each time)
    constexpr s16x16 k_0742(0.742f);
    constexpr s16x16 k_0677(0.677f);
    constexpr s16x16 k_0774(0.774f);
    constexpr s16x16 k_0710(0.710f);
    s16x16 cx_r1 = cx * k_0742;
    s16x16 cy_r1 = cy * k_0677;
    s16x16 cx_r2 = cx * k_0774;
    s16x16 cy_r2 = cy * k_0710;
 
    // Pre-multiply ts * frequency constants (was computing ts*k each time)
    constexpr s16x16 k_113(1.13f);
    constexpr s16x16 k_171(1.71f);
    constexpr s16x16 k_189(1.89f);
    constexpr s16x16 k_137(1.37f);
    constexpr s16x16 k_020(0.20f);
    constexpr s16x16 k_130(1.30f);
    constexpr s16x16 k_220(2.20f);
    constexpr s16x16 k_070(0.70f);
 
    s16x16 x1 = cx + cx_r1 * s16x16::sin(ts * k_113 + k_020);
    s16x16 y1 = cy + cy_r1 * s16x16::sin(ts * k_171 + k_130);
    s16x16 x2 = cx + cx_r2 * s16x16::sin(ts * k_189 + k_220);
    s16x16 y2 = cy + cy_r2 * s16x16::sin(ts * k_137 + k_070);
 
    drawAALine(x1, y1, x2, y2, t, mColorShift_fp);
 
    CRGB endA = rainbow(t, mColorShift_fp, s16x16());
    CRGB endB = rainbow(t, mColorShift_fp, half);
    constexpr s16x16 discDiam(1.7f);
    drawDot(x1, y1, discDiam, endA.r, endA.g, endA.b);
    drawDot(x2, y2, discDiam, endB.r, endB.g, endB.b);
}
 
void FlowFieldFP::emitOrbitalDots(s16x16 t) {
    int w = mState.width;
    int h = mState.height;
    int minDim = fl::min(w, h);
    int n = mParams.dot_count;
    constexpr s16x16 half(0.5f);
    constexpr s16x16 two_pi(6.2831853f);
    s16x16 fn = s16x16(n);
    s16x16 inv_fn = s16x16(1) / fn;  // Precompute reciprocal (was dividing per dot)
    s16x16 ocx = s16x16(w) * half - half;
    s16x16 ocy = s16x16(h) * half - half;
    s16x16 orad = s16x16(minDim) * s16x16(0.35f);
    s16x16 base = t * s16x16(3);
    constexpr s16x16 dotDiam(1.5f);
    s16x16 step = two_pi * inv_fn;  // Use precomputed reciprocal
 
    for (int i = 0; i < n; i++) {
        s16x16 a = base + s16x16(i) * step;
        s16x16 sin_a, cos_a;
        s16x16::sincos(a, sin_a, cos_a);
        s16x16 cx = ocx + cos_a * orad;
        s16x16 cy = ocy + sin_a * orad;
        CRGB c = rainbow(t, mColorShift_fp, s16x16(i) * inv_fn);
        drawDot(cx, cy, dotDiam, c.r, c.g, c.b);
    }
}
 
// ---------------------------------------------------------------------------
//  flowPrepare — fixed-point Perlin noise via perlin_s16x16::pnoise2d_raw()
// ---------------------------------------------------------------------------
 
void FlowFieldFP::flowPrepare(s16x16 t) {
    int w = mState.width;
    int h = mState.height;
    constexpr s16x16 kBaseFreq(0.23f);
    constexpr s16x16 neg_one(-1.0f);
    constexpr s16x16 one(1.0f);
 
    const i32 *fade_lut = mState.fade_lut;
 
    // Hoist loop-invariant products
    s16x16 freqX = kBaseFreq * mNoiseFreqX_fp;
    s16x16 scrollX = t * mFlowSpeedX_fp;  // time offset added to spatial coord
 
    for (int i = 0; i < w; i++) {
        s16x16 fx = s16x16(i) * freqX + scrollX;
        i32 noise_raw = perlin_s16x16::pnoise2d_raw(
            fx.raw(), 0, fade_lut, mPermX);
        s16x16 v = s16x16::from_raw(noise_raw);
        s16x16 clamped = s16x16::clamp(v * mFlowAmpX_fp, neg_one, one);
        mState.x_prof[i] = clamped.raw();
    }
 
    if (mParams.reverse_x_profile) {
        for (int i = 0; i < w / 2; i++) {
            i32 tmp = mState.x_prof[i];
            mState.x_prof[i] = mState.x_prof[w - 1 - i];
            mState.x_prof[w - 1 - i] = tmp;
        }
    }
 
    // Hoist loop-invariant products
    s16x16 freqY = kBaseFreq * mNoiseFreqY_fp;
    s16x16 scrollY = t * mFlowSpeedY_fp;  // time offset added to spatial coord
 
    for (int i = 0; i < h; i++) {
        s16x16 fx = s16x16(i) * freqY + scrollY;
        i32 noise_raw = perlin_s16x16::pnoise2d_raw(
            fx.raw(), 0, fade_lut, mPermY);
        s16x16 v = s16x16::from_raw(noise_raw);
        s16x16 clamped = s16x16::clamp(v * mFlowAmpY_fp, neg_one, one);
        mState.y_prof[i] = clamped.raw();
    }
 
    // Apply noise bias (attack/decay bumps from noisePunch triggers).
    for (int i = 0; i < w; i++) {
        i32 bias_raw = s16x16(mNoiseBias.getX(i)).raw();
        mState.x_prof[i] = clamp_q16(mState.x_prof[i] + bias_raw,
                                      neg_one.raw(), one.raw());
    }
    for (int i = 0; i < h; i++) {
        i32 bias_raw = s16x16(mNoiseBias.getY(i)).raw();
        mState.y_prof[i] = clamp_q16(mState.y_prof[i] + bias_raw,
                                      neg_one.raw(), one.raw());
    }
}
 
// ---------------------------------------------------------------------------
//  flowAdvect — Q16.16 hot path (~80% of frame time)
// ---------------------------------------------------------------------------
 
void FlowFieldFP::flowAdvect(i32 dt_raw) {
    int w = mState.width;
    int h = mState.height;
 
    // fade = pow(0.5, dt / halfLife) — pure fixed-point
    constexpr s16x16 half_fp(0.5f);
    constexpr s16x16 min_persistence(0.001f);
    s16x16 dt_fp = s16x16::from_raw(dt_raw);
    s16x16 halfLife_fp = mPersistence_fp.raw() > min_persistence.raw()
                             ? mPersistence_fp
                             : min_persistence;
    i32 fade_raw = s16x16::pow(half_fp, dt_fp / halfLife_fp).raw();
 
    i32 shift_raw = mFlowShift_fp.raw();
 
    i32 w_q16 = static_cast<i32>(w) << 16;
    i32 h_q16 = static_cast<i32>(h) << 16;
 
    // Grab raw array pointers — avoids vector operator[] overhead in hot loops.
    i32 *__restrict__ r  = mState.r.data();
    i32 *__restrict__ g  = mState.g.data();
    i32 *__restrict__ b  = mState.b.data();
    i32 *__restrict__ tr = mState.tr.data();
    i32 *__restrict__ tg = mState.tg.data();
    i32 *__restrict__ tb = mState.tb.data();
 
    // Pass 1: horizontal row shift
    // Lerp: a + ((b-a)*f >> 16) uses 1 i64 mul instead of 2.
    // Note: f = (-sh) & 0xFFFF is constant per row; the compiler hoists it at -O3.
    for (int y = 0; y < h; y++) {
        i32 sh = static_cast<i32>((static_cast<i64>(mState.y_prof[y]) * shift_raw) >> 16);
        int row_base = y * w;
        for (int x = 0; x < w; x++) {
            i32 sx_raw = (static_cast<i32>(x) << 16) - sh;
            // Modular wrap (single iteration sufficient for typical shift values)
            if (sx_raw < 0) sx_raw += w_q16;
            else if (sx_raw >= w_q16) sx_raw -= w_q16;
            // Safety for extreme shift values
            if (sx_raw < 0) sx_raw += w_q16;
            if (sx_raw >= w_q16) sx_raw -= w_q16;
 
            int ix0 = sx_raw >> 16;  // Already in [0, w-1] after wrap
            int ix1 = (ix0 + 1 < w) ? ix0 + 1 : 0;
            i32 f = sx_raw & 0xFFFF;
 
            int src0 = row_base + ix0;
            int src1 = row_base + ix1;
            int dst  = row_base + x;
 
            tr[dst] = r[src0] + static_cast<i32>((static_cast<i64>(r[src1] - r[src0]) * f) >> 16);
            tg[dst] = g[src0] + static_cast<i32>((static_cast<i64>(g[src1] - g[src0]) * f) >> 16);
            tb[dst] = b[src0] + static_cast<i32>((static_cast<i64>(b[src1] - b[src0]) * f) >> 16);
        }
    }
 
    // Pass 2: vertical column shift + fade
    // Note: f = (-sh) & 0xFFFF is constant per column; the compiler hoists it at -O3.
    for (int x = 0; x < w; x++) {
        i32 sh = static_cast<i32>((static_cast<i64>(mState.x_prof[x]) * shift_raw) >> 16);
        for (int y = 0; y < h; y++) {
            i32 sy_raw = (static_cast<i32>(y) << 16) - sh;
            if (sy_raw < 0) sy_raw += h_q16;
            else if (sy_raw >= h_q16) sy_raw -= h_q16;
            if (sy_raw < 0) sy_raw += h_q16;
            if (sy_raw >= h_q16) sy_raw -= h_q16;
 
            int iy0 = sy_raw >> 16;
            int iy1 = (iy0 + 1 < h) ? iy0 + 1 : 0;
            i32 f = sy_raw & 0xFFFF;
 
            int src0 = iy0 * w + x;
            int src1 = iy1 * w + x;
            int dst  = y * w + x;
 
            // Lerp + fade in two steps (2 i64 muls per channel instead of 3)
            i32 interp_r = tr[src0] + static_cast<i32>((static_cast<i64>(tr[src1] - tr[src0]) * f) >> 16);
            i32 interp_g = tg[src0] + static_cast<i32>((static_cast<i64>(tg[src1] - tg[src0]) * f) >> 16);
            i32 interp_b = tb[src0] + static_cast<i32>((static_cast<i64>(tb[src1] - tb[src0]) * f) >> 16);
 
            r[dst] = static_cast<i32>((static_cast<i64>(interp_r) * fade_raw) >> 16);
            g[dst] = static_cast<i32>((static_cast<i64>(interp_g) * fade_raw) >> 16);
            b[dst] = static_cast<i32>((static_cast<i64>(interp_b) * fade_raw) >> 16);
        }
    }
}
 
// ---------------------------------------------------------------------------
//  Flow vector overlay — fixed-point version
// ---------------------------------------------------------------------------
 
void FlowFieldFP::drawFlowVectors(fl::span<CRGB> leds) {
    int w = mState.width;
    int h = mState.height;
 
    // Helper: plot a single subpixel point via Tile2x2 through the XYMap.
    // Uses float for the interpolation math (unoptimized debug overlay).
    auto plotTile = [&](const CRGB &color, float px, float py,
                        bool horiz) {
        int ix = (int)floorf(px);
        int iy = (int)floorf(py);
        float fx = px - (float)ix;
        float fy = py - (float)iy;
        Tile2x2_u8 tile;
        tile.setOrigin((u16)ix, (u16)iy);
        if (horiz) {
            tile.at(0, 0) = (u8)((1.0f - fy) * 255.0f);
            tile.at(1, 0) = 0;
            tile.at(0, 1) = (u8)(fy * 255.0f);
            tile.at(1, 1) = 0;
        } else {
            tile.at(0, 0) = (u8)((1.0f - fx) * 255.0f);
            tile.at(1, 0) = (u8)(fx * 255.0f);
            tile.at(0, 1) = 0;
            tile.at(1, 1) = 0;
        }
        tile.draw(color, mXyMap, leds);
    };
 
    // Display amplitude: 0.3 = profile uses ~30% of the display around center.
    constexpr float amp = 0.3f;
 
    // X profile (per-column): plot value as y-position — cyan.
    // Interpolate between consecutive columns to fill gaps.
    CRGB xColor(0, 255, 255);
    float centerY = (float)(h - 1) * 0.5f;
    float prevPy = 0.0f;
    for (int x = 0; x < w; x++) {
        s16x16 val = s16x16::from_raw(mState.x_prof[x]); // [-1, 1]
        float py = centerY - val.to_float() * amp * centerY;
        plotTile(xColor, (float)x, py, true);
        if (x > 0) {
            float dy = py - prevPy;
            int gap = (int)fabsf(dy);
            for (int s = 1; s < gap; s++) {
                float t = (float)s / (float)gap;
                float midY = prevPy + dy * t;
                float midX = (float)(x - 1) + t;
                plotTile(xColor, midX, midY, true);
            }
        }
        prevPy = py;
    }
 
    // Y profile (per-row): plot value as x-position — yellow.
    // Interpolate between consecutive rows to fill gaps.
    CRGB yColor(255, 255, 0);
    float centerX = (float)(w - 1) * 0.5f;
    float prevPx = 0.0f;
    for (int y = 0; y < h; y++) {
        s16x16 val = s16x16::from_raw(mState.y_prof[y]); // [-1, 1]
        float px = centerX + val.to_float() * amp * centerX;
        plotTile(yColor, px, (float)y, false);
        if (y > 0) {
            float dx = px - prevPx;
            int gap = (int)fabsf(dx);
            for (int s = 1; s < gap; s++) {
                float t = (float)s / (float)gap;
                float midX = prevPx + dx * t;
                float midY = (float)(y - 1) + t;
                plotTile(yColor, midX, midY, false);
            }
        }
        prevPx = px;
    }
}
 
// ---------------------------------------------------------------------------
//  drawImpl — main entry point
// ---------------------------------------------------------------------------
 
void FlowFieldFP::drawImpl(DrawContext context, u32 dt_ms, u32 t_ms) {
    syncParams();
 
    s16x16 dt = s16x16(dt_ms * 0.001f);
    s16x16 t = s16x16(t_ms * 0.001f);
 
    i32 dt_raw = dt.raw();
 
    flowPrepare(t);
    switch (mParams.emitter_mode) {
    case 0:
        emitLissajousLine(t);
        break;
    case 1:
        emitOrbitalDots(t);
        break;
    case 2:
        emitLissajousLine(t);
        emitOrbitalDots(t);
        break;
    default:
        emitLissajousLine(t);
        break;
    }
    flowAdvect(dt_raw);
 
    // Output: convert Q16.16 grids to LED pixels
    int w = mState.width;
    int h = mState.height;
    const i32 *rp = mState.r.data();
    const i32 *gp = mState.g.data();
    const i32 *bp = mState.b.data();
    fl::span<CRGB> out = context.leds;
    for (int y = 0; y < h; y++) {
        int row_base = y * w;
        for (int x = 0; x < w; x++) {
            int i = row_base + x;
            u16 ledIdx = mXyMap.mapToIndex(static_cast<u16>(x), static_cast<u16>(y));
            out[ledIdx].r = q16_to_u8(rp[i]);
            out[ledIdx].g = q16_to_u8(gp[i]);
            out[ledIdx].b = q16_to_u8(bp[i]);
        }
    }
 
    if (mParams.show_flow_vectors) {
        drawFlowVectors(context.leds);
    }
}
 
} // namespace fl
 
FL_OPTIMIZATION_LEVEL_O3_END