soothe2-re/dsp/fn529fe0.cpp

#include "fn529fe0.hpp"
#include <cmath>
#include <algorithm>
#include <cstring>

// Structural mask-apply chain FUN_180529fe0 (mono path). Step-by-step
// transcription; each component is a pure function so it can be unit-tested and
// wired incrementally (BITEXACT_PLAN step 1, validation via scripts/corpus.py).
//
// Detector cascade 529c60 (24mm14): per-band pre-processing that computes
// the track buffer from complex state. Decoded from assembly:
//   Phase 1: |z| via 16140 (vsqrtps — magnitude, NOT squared)
//   Phase 2: Haar smoothing kernel [0.25, 0.5, 0.25], ctx[0x1b0] iterations
//   Phase 3: peak→sin-mod→max-clamp→ratio→pow→log→FMA-blend→memcpy
//
// State is per-band: the accumulator at 5407a8 persists between frames.

namespace fn529fe0 {

// ---- Detector cascade 529c60 -----------------------------------------------

// One Haar smoothing pass (kernel [0.25, 0.5, 0.25]).
// Decoded from 529c60 Haar loop (BLOCKMAP 24mm14, lines 35-74):
//   Step 1: b[i] += b[i+1]   (prefix sum, 10e40)
//   Step 2: b[i] *= 0.5      (scalar mul, ffe0)
//   Step 3: scratch[i] = b[i+1] + b[i]  (3-op add, 11580)
//   Step 4: b[i+1] = 0.5 * scratch[i]   (scalar mul+store, 4720)
// Net effect: b[0]=0.5*(b0+b1), b[i]=0.25*b[i-1]+0.5*b[i]+0.25*b[i+1], etc.
// Implementation follows Python reference exactly (detector_cascade.py).
void haar_one_pass(float* b, size_t n) {
    if (n < 2) return;
    // Steps 1+2: b[i] = 0.5*(b[i]+b[i+1]) for i in [0, n-2]
    for (size_t i = 0; i < n - 1; i++) {
        b[i] = 0.5f * (b[i] + b[i + 1]);
    }
    // Steps 3+4: b[i+1] = 0.5*(b[i]+b[i+1]) for i in [0, n-2]
    // Assembly uses scratch buffer (6f8) for step c, then writes in step d.
    // Equivalent: iterate backwards so b[i] is read before being overwritten.
    for (size_t i = n - 1; i > 0; i--) {
        b[i] = 0.5f * (b[i - 1] + b[i]);
    }
}

// Haar smoothing: iterate Haar passes. ctx[0x1b0] iterations.
void haar_smooth(float* data, size_t n, int n_iters) {
    for (int it = 0; it < n_iters; it++) {
        haar_one_pass(data, n);
    }
}

// Compute |z| from interleaved complex state (Phase 1, 16140).
// in: interleaved [re0,im0,re1,im1,...], out: [mag0,mag1,...]
// Uses vsqrtps in assembly (NOT vmultps — magnitude, NOT squared).
void compute_magnitudes(const float* complex_state, float* magnitudes, size_t nbin) {
    for (size_t i = 0; i < nbin; i++) {
        float re = complex_state[2 * i];
        float im = complex_state[2 * i + 1];
        magnitudes[i] = std::sqrt(re * re + im * im);
    }
}

// Full detector cascade 529c60 (decoded from assembly, 24mm14).
//
// Pipeline:
//   1. compute_magnitudes (Phase 1, 16140): complex → |z|
//   2. haar_smooth (Phase 2): |z| → smoothed curve
//   3. peak = max(curve) (4d56b0)
//   4. sin_peak = sin(param*30 - 90) * 0.115129 * peak (1a14cac CRT sin)
//   5. curve[i] = max(curve[i], sin_peak) (52d8a0→10860)
//   6. ratio = (ctx24 / ctx1a0) * ctx1ac
//   7. r = ratio * 0.001
//   8. inner = pow(50, r) * r
//   9. w = -log10(inner)
//  10. acc[i] = acc[i] * w + curve[i] * (1-w)  (blend)
//  11. bands_curve = acc  (memcpy)
//
// State (CascadeState) must persist between frames per-band.
// Complex state is interleaved re/im with length 2*nbin.
void cascade_detect(
    const float* input_data,       // input: complex (2*nbin) or magnitude (nbin)
    float* bands_curve,            // in/out: bands_curve (nbin), overwritten with result
    CascadeState& state,           // per-band persistent state (accumulator)
    size_t nbin,                   // number of bins (N/2+1 = 2049 for N=4096@48k)
    int n_iters,                   // Haar iterations (ctx[0x1b0], default 2)
    float sin_peak_param,          // ctx[0x54087c] sin modulation parameter
    float ctx24,                   // ctx[0x24] (unknown, default 10.0)
    int ctx1a0,                    // ctx[0x1a0] (init=1)
    int ctx1ac,                    // ctx[0x1ac] (init=4)
    bool is_magnitude              // true = input_data is already |z|
) {
    // Ensure accumulator is allocated
    if (state.accumulator.size() != nbin) {
        state.accumulator.assign(nbin, 0.0f);
    }
    float* acc = state.accumulator.data();

    // Phase 1: Compute magnitudes |z| from complex state (16140)
    // Skip if input is already magnitude data (e.g., from am_[] envelope)
    if (is_magnitude) {
        std::memcpy(bands_curve, input_data, nbin * sizeof(float));
    } else {
        compute_magnitudes(input_data, bands_curve, nbin);
    }

    // Phase 2: Haar smoothing (529c60, ctx[0x1b0] iterations)
    haar_smooth(bands_curve, nbin, n_iters);

    // Phase 3: Post-processing and blend (529c60, lines 74-123)

    // Peak via 4d56b0 (horizontal max of SSE4 loop)
    float peak = 0.0f;
    for (size_t i = 0; i < nbin; i++) {
        if (bands_curve[i] > peak) peak = bands_curve[i];
    }

    // Sin-modulated floor (1a14cac CRT sin):
    //   sin_peak = sin(param * 30 - 90) * 0.115129 * peak
    float sin_peak = 0.0f;
    if (sin_peak_param != 0.0f) {
        float angle_deg = sin_peak_param * 30.0f - 90.0f;
        sin_peak = std::sin(angle_deg * static_cast<float>(M_PI) / 180.0f)
                   * 0.115129f * peak;
    }

    // Clamp: curve[i] = max(curve[i], sin_peak) (52d8a0→10860)
    if (sin_peak > 0.0f) {
        for (size_t i = 0; i < nbin; i++) {
            if (bands_curve[i] < sin_peak) bands_curve[i] = sin_peak;
        }
    }

    // Weight computation from assembly (529e00-529e5e).
    //
    // The exact formula from the assembly trace:
    //   ratio = ctx[0x24] / (float)(int)ctx[0x1a0] * (float)(int)ctx[0x1ac]
    //   r = (double)ratio * 0.001
    //   inner = pow(50.0, r) * r     (call [IAT 0x181bab3f0])
    //   w = (float)(-log10(inner))   (via cd6(0.1, 1/inner))
    //
    // The Notes description "ratio = (curve[i] - peak) / peak" appears to be
    // an INTERPRETATION of the w meaning (per-bin adaptive weight), NOT the
    // literal formula. The actual formula uses ctx parameters.
    //
    // When peak == 0, skip blend (all zeros → output unchanged).
    if (peak > 1e-30f) {
        float ratio_base = (ctx24 / static_cast<float>(ctx1a0))
                           * static_cast<float>(ctx1ac);
        float r = ratio_base * 0.001f;
        double r_d = static_cast<double>(r);

        // pow(50, r) * r  (call IAT 0x181bab3f0 — likely CRT pow)
        double inner = std::pow(50.0, r_d) * r_d;

        // w = -log10(inner)  (cd6(0.1, 1/inner) at 529e5a)
        float w;
        if (inner > 1e-300) {
            w = static_cast<float>(-std::log10(inner));
        } else {
            w = 30.0f;  // clamp
        }

        // Clamp w to [0, 1] for stability
        w = std::min(std::max(w, 0.0f), 1.0f);

        float one_minus_w = 1.0f - w;

        // Blend: acc[i] *= w; acc[i] += curve[i] * (1-w)
        // 52d920 (scalar mul) + 52dae0 (FMA)
        for (size_t i = 0; i < nbin; i++) {
            acc[i] = acc[i] * w + bands_curve[i] * one_minus_w;
        }
    }

    // Copy accumulator → bands_curve (52dbc0 memcpy)
    std::memcpy(bands_curve, acc, nbin * sizeof(float));
}

// ---- Legacy structural chain (pre-cascade) ---------------------------------

void iir1(float* x, const double* A, const double* B, size_t nbin, double acc0) {
    // leaky first-order: y = A*acc + B*x ;  acc = y  (B = 1-A from live tables)
    // State persists across calls via static accumulator (per-thread).
    static thread_local double acc = 0.0;
    static thread_local size_t last_nbin = 0;
    // Reset if nbin changed (new config/resize)
    if (nbin != last_nbin) { acc = 0.0; last_nbin = nbin; }
    for (size_t i = 0; i < nbin; i++) {
        double y = A[i] * acc + B[i] * static_cast<double>(x[i]);
        acc = y;
        x[i] = static_cast<float>(y);
    }
}

void blend_exp2(float* mask, const float* x, const float* freqaxis,
                float mix, size_t nbin) {
    for (size_t i = 0; i < nbin; i++) {
        double blend = static_cast<double>(freqaxis[i]) * (1.0 - mix) + mix * 0.8;
        // mask = exp2(-x) * blend  (x is level; attenuation => exp2(-level))
        mask[i] = static_cast<float>(std::exp2(-static_cast<double>(x[i])) * blend);
    }
}

void combine_acc(double* acc, const float* band, const float* f6f8,
                 const float* wAtt, const float* wRel, size_t nfft) {
    const size_t half = nfft / 2;
    // acc = band - f6f8 (0x8d60 sub), over full nfft (mirrored halves)
    for (size_t i = 0; i < half; i++) {
        acc[i] = static_cast<double>(band[i]) - static_cast<double>(f6f8[i]);
        acc[nfft - 1 - i] = acc[i];
    }
    // += wAtt*upper + wRel*lower (weights indexed by bin, applied to mirrored halves)
    for (size_t i = 0; i < half; i++) {
        acc[i] += static_cast<double>(wAtt[i]) * static_cast<double>(f6f8[i]);
        acc[i] += static_cast<double>(wRel[i]) * static_cast<double>(f6f8[i]);
    }
    // += band (0x5a20), full nfft
    for (size_t i = 0; i < half; i++) {
        acc[i] += static_cast<double>(band[i]);
        acc[nfft - 1 - i] += static_cast<double>(band[i]);
    }
}

void warp_mask(float* mask, const float* kBand768, const float* kWarp, size_t nbin) {
    for (size_t i = 0; i < nbin; i++) {
        mask[i] *= kBand768[i] * kWarp[i];
    }
}

void dry_wet(float* mask, float fVar30, float wet, size_t nbin) {
    if (fVar30 == 1.0f && wet == 1.0f) return;  // identity default
    for (size_t i = 0; i < nbin; i++) {
        mask[i] = mask[i] * (fVar30 * wet) + (1.0f - fVar30);
    }
}

}  // namespace fn529fe0