minidsp.c

Go to the documentation of this file.

 
#include "minidsp.h"
 
/* -----------------------------------------------------------------------
 * Static (file-scope) variables for FFT caching
 *
 * Creating FFTW plans is expensive, so we cache them.  As long as the
 * signal length N stays the same between calls, we reuse the existing
 * plans and buffers.  If N changes, we tear everything down and rebuild.
 * -----------------------------------------------------------------------*/
 
static unsigned      _N        = 0;    /* Current cached signal length        */
static double       *siga_loc  = nullptr; /* Local copy of input signal A        */
static double       *sigb_loc  = nullptr; /* Local copy of input signal B        */
static double       *lags_loc  = nullptr; /* Shifted cross-correlation output    */
static fftw_complex *ffta      = nullptr; /* FFT of signal A                     */
static fftw_complex *fftb      = nullptr; /* FFT of signal B                     */
static fftw_complex *xspec     = nullptr; /* Cross-spectrum (FFT_B * conj(FFT_A))*/
static double       *xcorr     = nullptr; /* Raw cross-correlation (time domain) */
 
static fftw_plan     pa = nullptr;        /* FFTW plan: signal A -> FFT          */
static fftw_plan     pb = nullptr;        /* FFTW plan: signal B -> FFT          */
static fftw_plan     px = nullptr;        /* FFTW plan: cross-spectrum -> IFFT   */
 
/* -----------------------------------------------------------------------
 * Internal helpers for FFT buffer management
 * -----------------------------------------------------------------------*/
 
static void _xcorr_free(void)
{
    if (xspec)    fftw_free(xspec);
    if (fftb)     fftw_free(fftb);
    if (ffta)     fftw_free(ffta);
 
    if (lags_loc) free(lags_loc);
    if (xcorr)    free(xcorr);
    if (sigb_loc) free(sigb_loc);
    if (siga_loc) free(siga_loc);
 
    xspec = nullptr;   fftb = nullptr;    ffta = nullptr;
    lags_loc = nullptr; xcorr = nullptr;  sigb_loc = nullptr; siga_loc = nullptr;
}
 
static void _xcorr_malloc(void)
{
    siga_loc = calloc(_N, sizeof(double));
    sigb_loc = calloc(_N, sizeof(double));
    xcorr    = calloc(_N + 1, sizeof(double)); /* FFTW c2r needs N+1 */
    lags_loc = calloc(_N, sizeof(double));
 
    /* FFTW provides its own allocator that guarantees proper alignment
     * for SIMD instructions (SSE2, AVX, etc.).  Always use it for
     * fftw_complex arrays. */
    ffta  = fftw_alloc_complex(_N);
    fftb  = fftw_alloc_complex(_N);
    xspec = fftw_alloc_complex(_N);
}
 
static void _xcorr_teardown(void)
{
    if (pa) fftw_destroy_plan(pa);
    if (pb) fftw_destroy_plan(pb);
    if (px) fftw_destroy_plan(px);
    pa = nullptr; pb = nullptr; px = nullptr;
 
    _xcorr_free();
}
 
static void _xcorr_setup(void)
{
    _xcorr_teardown();
    _xcorr_malloc();
 
    /* FFTW_ESTIMATE: pick a reasonable plan quickly (don't benchmark).
     * FFTW_DESTROY_INPUT: FFTW may overwrite the input array -- that's
     * fine because we always copy the data into our local buffers first. */
    pa = fftw_plan_dft_r2c_1d(_N, siga_loc, ffta,  FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    pb = fftw_plan_dft_r2c_1d(_N, sigb_loc, fftb,  FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    px = fftw_plan_dft_c2r_1d(_N, xspec,    xcorr, FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
}
 
static void _gcc_setup(unsigned N)
{
    if (_N != N) {
        _N = N;
        _xcorr_setup();
    }
}
 
/* -----------------------------------------------------------------------
 * Static (file-scope) variables for spectrum analysis caching
 *
 * These are separate from the GCC cache above so that spectrum analysis
 * and delay estimation can be used independently without invalidating
 * each other's plans.
 * -----------------------------------------------------------------------*/
 
static unsigned      _spec_N       = 0;       /* Cached signal length      */
static double       *_spec_in      = nullptr;  /* Local copy of input       */
static fftw_complex *_spec_out     = nullptr;  /* FFT output                */
static fftw_plan     _spec_plan    = nullptr;  /* FFTW r2c plan             */
 
/* -----------------------------------------------------------------------
 * Static variables for STFT Hanning window cache
 *
 * The STFT reuses the shared _spec_* r2c plan above.  Only the Hanning
 * window buffer is separate, since different callers may use different N.
 * -----------------------------------------------------------------------*/
 
static double  *_stft_win   = nullptr; /* Cached Hanning window             */
static unsigned _stft_win_N = 0;       /* Window length corresponding to above */
 
static void _spec_free(void)
{
    if (_spec_out) fftw_free(_spec_out);
    if (_spec_in)  free(_spec_in);
    _spec_out = nullptr;
    _spec_in  = nullptr;
}
 
static void _spec_teardown(void)
{
    if (_spec_plan) fftw_destroy_plan(_spec_plan);
    _spec_plan = nullptr;
    _spec_free();
}
 
static void _spec_setup(unsigned N)
{
    if (_spec_N == N) return;
 
    _spec_teardown();
    _spec_N = N;
 
    _spec_in  = calloc(N, sizeof(double));
    _spec_out = fftw_alloc_complex(N / 2 + 1);
 
    _spec_plan = fftw_plan_dft_r2c_1d(
        (int)N, _spec_in, _spec_out,
        FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
}
 
static void _stft_teardown(void)
{
    free(_stft_win);
    _stft_win   = nullptr;
    _stft_win_N = 0;
}
 
static void _stft_setup(unsigned N)
{
    _spec_setup(N);                 /* reuse shared r2c plan */
    if (_stft_win_N == N) return;   /* fast path: window already correct */
 
    free(_stft_win);
    _stft_win = malloc(N * sizeof(double));
    assert(_stft_win != nullptr);
    MD_Gen_Hann_Win(_stft_win, N);
    _stft_win_N = N;
}
 
/* -----------------------------------------------------------------------
 * Internal helpers for cross-correlation post-processing
 * -----------------------------------------------------------------------*/
 
static void _fftshift(const double *in, double *out, unsigned N)
{
    /* The split point: how many elements are in the "second half" */
    unsigned half = (unsigned)floor((double)N / 2.0);
 
    /* Copy the second half of in[] (negative lags) to the start of out[] */
    memcpy(out, &in[half], sizeof(double) * (N - half));
 
    /* Copy the first half of in[] (positive lags) to the end of out[] */
    memcpy(&out[N - half], in, sizeof(double) * half);
}
 
static void _max_index(const double *a, unsigned N,
                       double *max, unsigned *maxi)
{
    assert(a != nullptr);
    assert(N >= 1);
 
    unsigned best_i = 0;
    double   best_v = a[0];
 
    for (unsigned i = 1; i < N; i++) {
        if (a[i] > best_v) {
            best_v = a[i];
            best_i = i;
        }
    }
 
    *max  = best_v;
    *maxi = best_i;
}
 
/* -----------------------------------------------------------------------
 * Public API: resource cleanup
 * -----------------------------------------------------------------------*/
 
void MD_shutdown(void)
{
    _xcorr_teardown();
    _stft_teardown();
    _spec_teardown();
    _spec_N = 0;
    fftw_cleanup();
}
 
/* -----------------------------------------------------------------------
 * Public API: basic signal measurements
 * -----------------------------------------------------------------------*/
 
double MD_dot(const double *a, const double *b, unsigned N)
{
    double d = 0.0;
    for (unsigned i = 0; i < N; i++) {
        d += a[i] * b[i];
    }
    return d;
}
 
double MD_energy(const double *a, unsigned N)
{
    assert(a != nullptr);
    if (N == 1) return a[0] * a[0];
    return MD_dot(a, a, N);
}
 
double MD_power(const double *a, unsigned N)
{
    assert(a != nullptr);
    assert(N > 0);
    return MD_energy(a, N) / (double)N;
}
 
double MD_power_db(const double *a, unsigned N)
{
    assert(a != nullptr);
    assert(N > 0);
    double p = fmax(1.0e-10, MD_power(a, N));
    return 10.0 * log10(p);
}
 
/* -----------------------------------------------------------------------
 * Public API: signal scaling and conditioning
 * -----------------------------------------------------------------------*/
 
double MD_scale(double in,
                double oldmin, double oldmax,
                double newmin, double newmax)
{
    return (in - oldmin) * (newmax - newmin) / (oldmax - oldmin) + newmin;
}
 
void MD_scale_vec(double *in, double *out, unsigned N,
                  double oldmin, double oldmax,
                  double newmin, double newmax)
{
    assert(in != nullptr);
    assert(out != nullptr);
    assert(oldmin < oldmax);
    assert(newmin < newmax);
    if (N == 0) return;
 
    double scale = (newmax - newmin) / (oldmax - oldmin);
    for (unsigned i = 0; i < N; i++) {
        out[i] = (in[i] - oldmin) * scale + newmin;
    }
}
 
void MD_fit_within_range(double *in, double *out, unsigned N,
                         double newmin, double newmax)
{
    assert(in != nullptr);
    assert(out != nullptr);
    assert(newmin < newmax);
    if (N == 0) return;
 
    /* Find the actual min and max of the input */
    double in_min = in[0];
    double in_max = in[0];
    for (unsigned i = 1; i < N; i++) {
        if (in[i] < in_min) in_min = in[i];
        if (in[i] > in_max) in_max = in[i];
    }
 
    if (in_min >= newmin && in_max <= newmax) {
        /* Already fits -- just copy without scaling */
        for (unsigned i = 0; i < N; i++) {
            out[i] = in[i];
        }
    } else {
        MD_scale_vec(in, out, N, in_min, in_max, newmin, newmax);
    }
}
 
void MD_adjust_dblevel(const double *in, double *out,
                       unsigned N, double dblevel)
{
    /* Convert the target dB level back to linear power */
    double desired_power = pow(10.0, dblevel / 10.0);
 
    /* Compute the gain factor:
     * We want:  (1/N) * sum(out[i]^2) = desired_power
     * Since out[i] = in[i] * gain:
     *   (gain^2 / N) * sum(in[i]^2) = desired_power
     *   gain = sqrt(desired_power * N / energy_in) */
    double input_energy = MD_energy(in, N);
    double gain = sqrt((desired_power * (double)N) / input_energy);
 
    /* Apply the gain and check for out-of-range values */
    bool out_of_range = false;
    for (unsigned i = 0; i < N; i++) {
        out[i] = in[i] * gain;
        if (out[i] > 1.0 || out[i] < -1.0) {
            out_of_range = true;
        }
    }
 
    /* If any samples exceed [-1.0, 1.0], rescale everything to fit */
    if (out_of_range) {
        MD_fit_within_range(out, out, N, -1.0, 1.0);
    }
}
 
double MD_entropy(const double *a, unsigned N, bool clip)
{
    assert(a != nullptr);
 
    if (N <= 1) return 0.0;
 
    /* Maximum possible entropy for N bins (uniform distribution) */
    double max_entropy = log2((double)N);
 
    /* First pass: compute the total (for normalisation into probabilities) */
    double total = 0.0;
    if (clip) {
        for (unsigned i = 0; i < N; i++) {
            total += (a[i] < 0.0) ? 0.0 : a[i];
        }
    } else {
        for (unsigned i = 0; i < N; i++) {
            total += a[i] * a[i];
        }
    }
 
    /* If the total is zero (e.g., all-zeros input), entropy is undefined.
     * We return 0.0 because there is no meaningful distribution. */
    if (total == 0.0) return 0.0;
 
    /* Second pass: compute H = -SUM( p_i * log2(p_i) ) */
    double entropy = 0.0;
    for (unsigned i = 0; i < N; i++) {
        if (a[i] == 0.0) continue;
        if (clip && a[i] < 0.0) continue;
 
        double p;
        if (clip) {
            p = a[i] / total;
        } else {
            p = (a[i] * a[i]) / total;
        }
 
        entropy += p * log2(p);  /* Note: p*log2(p) is always <= 0 */
    }
 
    /* Negate and normalise so the result is in [0, 1] */
    return -entropy / max_entropy;
}
 
/* -----------------------------------------------------------------------
 * Public API: window generation
 * -----------------------------------------------------------------------*/
 
void MD_Gen_Hann_Win(double *out, unsigned n)
{
    double n_minus_1 = (double)(n - 1);
    for (unsigned i = 0; i < n; i++) {
        out[i] = 0.5 * (1.0 - cos(2.0 * M_PI * (double)i / n_minus_1));
    }
}
 
/* -----------------------------------------------------------------------
 * Public API: Signal generators
 * -----------------------------------------------------------------------*/
 
void MD_sine_wave(double *output, unsigned N, double amplitude,
                  double freq, double sample_rate)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(sample_rate > 0.0);
    double phase_step = 2.0 * M_PI * freq / sample_rate;
    for (unsigned i = 0; i < N; i++)
        output[i] = amplitude * sin(phase_step * (double)i);
}
 
void MD_white_noise(double *output, unsigned N, double amplitude,
                    unsigned seed)
{
    assert(output != nullptr);
    assert(N > 0);
 
    /* 64-bit LCG (Knuth MMIX constants) for uniform doubles in (0, 1).
     * Self-contained so we don't need POSIX drand48. */
    unsigned long long state = (unsigned long long)seed;
 
    /* Box-Muller transform: convert pairs of uniform random numbers
     * into pairs of Gaussian random numbers (mean 0, std dev 1),
     * then scale by the requested amplitude.
     *
     * For each pair (u1, u2) drawn from (0, 1]:
     *   z0 = sqrt(-2 ln u1) * cos(2 pi u2)
     *   z1 = sqrt(-2 ln u1) * sin(2 pi u2) */
    unsigned i = 0;
    while (i + 1 < N) {
        state = state * 6364136223846793005ULL + 1442695040888963407ULL;
        double u1 = (double)(state >> 11) * 0x1.0p-53;
        if (u1 < DBL_MIN) u1 = DBL_MIN;
        state = state * 6364136223846793005ULL + 1442695040888963407ULL;
        double u2 = (double)(state >> 11) * 0x1.0p-53;
 
        double r = sqrt(-2.0 * log(u1));
        double theta = 2.0 * M_PI * u2;
        output[i]     = amplitude * r * cos(theta);
        output[i + 1] = amplitude * r * sin(theta);
        i += 2;
    }
    /* If N is odd, generate one extra pair and use only the first value */
    if (i < N) {
        state = state * 6364136223846793005ULL + 1442695040888963407ULL;
        double u1 = (double)(state >> 11) * 0x1.0p-53;
        if (u1 < DBL_MIN) u1 = DBL_MIN;
        state = state * 6364136223846793005ULL + 1442695040888963407ULL;
        double u2 = (double)(state >> 11) * 0x1.0p-53;
        output[i] = amplitude * sqrt(-2.0 * log(u1)) * cos(2.0 * M_PI * u2);
    }
}
 
void MD_impulse(double *output, unsigned N, double amplitude, unsigned position)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(position < N);
    memset(output, 0, N * sizeof(double));
    output[position] = amplitude;
}
 
void MD_chirp_linear(double *output, unsigned N, double amplitude,
                     double f_start, double f_end, double sample_rate)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(sample_rate > 0.0);
 
    if (N == 1) {
        output[0] = 0.0;
        return;
    }
 
    double T = (double)(N - 1) / sample_rate;
    double chirp_rate = (f_end - f_start) / T;
 
    for (unsigned i = 0; i < N; i++) {
        double t = (double)i / sample_rate;
        double phase = 2.0 * M_PI * (f_start * t + 0.5 * chirp_rate * t * t);
        output[i] = amplitude * sin(phase);
    }
}
 
void MD_chirp_log(double *output, unsigned N, double amplitude,
                  double f_start, double f_end, double sample_rate)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(sample_rate > 0.0);
    assert(f_start > 0.0);
    assert(f_end > 0.0);
    assert(f_start != f_end);
 
    if (N == 1) {
        output[0] = 0.0;
        return;
    }
 
    double T = (double)(N - 1) / sample_rate;
    double ratio = f_end / f_start;
    double log_ratio = log(ratio);
 
    for (unsigned i = 0; i < N; i++) {
        double t = (double)i / sample_rate;
        double phase = 2.0 * M_PI * f_start * T
                     * (pow(ratio, t / T) - 1.0) / log_ratio;
        output[i] = amplitude * sin(phase);
    }
}
 
void MD_square_wave(double *output, unsigned N, double amplitude,
                    double freq, double sample_rate)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(sample_rate > 0.0);
    double phase_step = 2.0 * M_PI * freq / sample_rate;
    for (unsigned i = 0; i < N; i++) {
        double phase = fmod(phase_step * (double)i, 2.0 * M_PI);
        if (phase < 0.0) phase += 2.0 * M_PI;
        if (phase == 0.0 || phase == M_PI)
            output[i] = 0.0;
        else if (phase < M_PI)
            output[i] = amplitude;
        else
            output[i] = -amplitude;
    }
}
 
void MD_sawtooth_wave(double *output, unsigned N, double amplitude,
                      double freq, double sample_rate)
{
    assert(output != nullptr);
    assert(N > 0);
    assert(sample_rate > 0.0);
    double phase_step = 2.0 * M_PI * freq / sample_rate;
    for (unsigned i = 0; i < N; i++) {
        double phase = fmod(phase_step * (double)i, 2.0 * M_PI);
        if (phase < 0.0) phase += 2.0 * M_PI;
        output[i] = amplitude * (phase / M_PI - 1.0);
    }
}
 
/* -----------------------------------------------------------------------
 * Public API: FFT / Spectrum Analysis
 * -----------------------------------------------------------------------*/
 
void MD_magnitude_spectrum(const double *signal, unsigned N, double *mag_out)
{
    assert(signal != nullptr);
    assert(mag_out != nullptr);
    assert(N >= 2);
 
    _spec_setup(N);
 
    /* Copy input into the local buffer (FFTW may overwrite it) */
    memcpy(_spec_in, signal, N * sizeof(double));
 
    /* Execute the forward FFT (real -> complex) */
    fftw_execute(_spec_plan);
 
    /* Compute magnitude |X(k)| = sqrt(re^2 + im^2) for each bin */
    unsigned num_bins = N / 2 + 1;
    for (unsigned k = 0; k < num_bins; k++) {
        mag_out[k] = cabs(_spec_out[k]);
    }
}
 
void MD_power_spectral_density(const double *signal, unsigned N, double *psd_out)
{
    assert(signal != nullptr);
    assert(psd_out != nullptr);
    assert(N >= 2);
 
    _spec_setup(N);
 
    /* Copy input into the local buffer (FFTW may overwrite it) */
    memcpy(_spec_in, signal, N * sizeof(double));
 
    /* Execute the forward FFT (real -> complex) */
    fftw_execute(_spec_plan);
 
    /* Compute PSD[k] = |X(k)|^2 / N = (re^2 + im^2) / N.
     * We use creal/cimag instead of cabs to avoid a redundant sqrt --
     * cabs computes sqrt(re^2 + im^2), and we'd just square it again. */
    unsigned num_bins = N / 2 + 1;
    for (unsigned k = 0; k < num_bins; k++) {
        double re = creal(_spec_out[k]);
        double im = cimag(_spec_out[k]);
        psd_out[k] = (re * re + im * im) / (double)N;
    }
}
 
void MD_phase_spectrum(const double *signal, unsigned N, double *phase_out)
{
    assert(signal    != nullptr);
    assert(phase_out != nullptr);
    assert(N >= 2);
 
    _spec_setup(N);
 
    /* Copy input into the local buffer (FFTW may overwrite it) */
    memcpy(_spec_in, signal, N * sizeof(double));
 
    /* Execute the forward FFT (real -> complex) */
    fftw_execute(_spec_plan);
 
    /* Compute phi(k) = atan2(Im(X(k)), Re(X(k))) for each bin.
     * carg() from <complex.h> does exactly this and returns [-pi, pi]. */
    unsigned num_bins = N / 2 + 1;
    for (unsigned k = 0; k < num_bins; k++) {
        phase_out[k] = carg(_spec_out[k]);
    }
}
 
unsigned MD_stft_num_frames(unsigned signal_len, unsigned N, unsigned hop)
{
    if (signal_len < N) return 0;
    return (signal_len - N) / hop + 1;
}
 
void MD_stft(const double *signal, unsigned signal_len,
             unsigned N, unsigned hop, double *mag_out)
{
    assert(signal  != nullptr);
    assert(mag_out != nullptr);
    assert(N   >= 2);
    assert(hop >= 1);
 
    unsigned num_frames = MD_stft_num_frames(signal_len, N, hop);
    if (num_frames == 0) return;   /* signal too short for even one frame */
 
    _stft_setup(N);
    unsigned num_bins = N / 2 + 1;
 
    for (unsigned f = 0; f < num_frames; f++) {
        const double *src = signal + (size_t)f * hop;  /* (size_t) avoids overflow */
 
        /* Apply Hanning window directly into the shared input buffer.
         * Touching every element anyway, so no separate memcpy is needed. */
        for (unsigned n = 0; n < N; n++) {
            _spec_in[n] = src[n] * _stft_win[n];
        }
 
        fftw_execute(_spec_plan);
 
        /* Write magnitude row: |X_f(k)| = sqrt(re^2 + im^2).
         * cabs() is correct here -- we need the actual sqrt magnitude,
         * not |z|^2, so the creal/cimag shortcut from PSD does not apply. */
        double *row = mag_out + (size_t)f * num_bins;
        for (unsigned k = 0; k < num_bins; k++) {
            row[k] = cabs(_spec_out[k]);
        }
    }
}
 
/* -----------------------------------------------------------------------
 * Public API: GCC-PHAT delay estimation
 * -----------------------------------------------------------------------*/
 
void MD_get_multiple_delays(const double **sigs, unsigned M, unsigned N,
                            unsigned margin, int weightfunc,
                            int *outdelays)
{
    /* Static buffers are reused across calls for efficiency.
     * They are only re-allocated when the signal length changes. */
    static unsigned last_N   = 0;
    static double  *hann_win = nullptr;
    static double  *t_ref    = nullptr;
    static double  *t_sig    = nullptr;
 
    if (M < 2) return;
 
    /* Re-allocate if the signal length has changed */
    if (last_N != N) {
        free(hann_win);  /* free(nullptr) is safe in C */
        free(t_ref);
        free(t_sig);
 
        hann_win = malloc(N * sizeof(double));
        assert(hann_win != nullptr);
        MD_Gen_Hann_Win(hann_win, N);
 
        t_ref = malloc(N * sizeof(double));
        assert(t_ref != nullptr);
 
        t_sig = malloc(N * sizeof(double));
        assert(t_sig != nullptr);
 
        last_N = N;
    }
 
    /* Apply the Hanning window to the reference signal */
    for (unsigned i = 0; i < N; i++) {
        t_ref[i] = sigs[0][i] * hann_win[i];
    }
 
    /* Compute the delay for each non-reference signal */
    for (unsigned i = 0; i < M - 1; i++) {
        /* Window the comparison signal */
        for (unsigned j = 0; j < N; j++) {
            t_sig[j] = sigs[i + 1][j] * hann_win[j];
        }
        outdelays[i] = MD_get_delay(t_ref, t_sig, N, nullptr, margin, weightfunc);
    }
}
 
int MD_get_delay(const double *siga, const double *sigb, unsigned N,
                 double *ent, unsigned margin, int weightfunc)
{
    _gcc_setup(N);
 
    /* Compute the full cross-correlation */
    MD_gcc(siga, sigb, N, lags_loc, weightfunc);
 
    /* The zero-lag position in the shifted output */
    unsigned center = (unsigned)ceil((double)N / 2.0);
 
    /* Clamp the margin so we don't read outside the array */
    unsigned m = margin;
    if (center < m) {
        m = center;
    }
    if (center + m >= N) {
        m = (N - 1) - center;
    }
 
    /* Search the window [center-m, center+m] for the peak */
    unsigned start = center - m;
    unsigned len   = 2 * m + 1;
    double   peak_val;
    unsigned peak_i;
 
    _max_index(lags_loc + start, len, &peak_val, &peak_i);
 
    /* Optionally compute the entropy of the search window */
    if (ent != nullptr) {
        *ent = MD_entropy(lags_loc + start, len, true);
    }
 
    /* Convert the peak index to a signed delay relative to zero-lag */
    return (int)(peak_i) - (int)(m);
}
 
void MD_gcc(const double *siga, const double *sigb, unsigned N,
            double *lagvals, int weightfunc)
{
    _gcc_setup(N);
 
    /* Copy input into local buffers (FFTW may overwrite them) */
    memcpy(siga_loc, siga, _N * sizeof(double));
    memcpy(sigb_loc, sigb, _N * sizeof(double));
 
    /* Forward FFT of both signals.
     *
     * For a real-valued input of length N, the FFT output has only
     * N/2 + 1 unique complex values (the rest are conjugate-symmetric).
     * FFTW's r2c transform exploits this and only writes N/2+1 values. */
    fftw_execute(pa);
    fftw_execute(pb);
 
    /* Compute the weighted cross-spectrum.
     *
     * The cross-spectrum is:  X[k] = FFT_B[k] * conj(FFT_A[k])
     *
     * For PHAT weighting, we divide by the magnitude of the cross-spectrum.
     * This keeps only the phase information, which produces a much sharper
     * peak in the time-domain correlation.
     *
     * We only loop over the N/2+1 non-redundant frequency bins. */
    unsigned num_bins = _N / 2 + 1;
 
    switch (weightfunc) {
    case PHAT:
        for (unsigned i = 0; i < num_bins; i++) {
            double complex cs = fftb[i] * conj(ffta[i]);
            /* Divide by magnitude; add DBL_MIN to prevent division by zero */
            xspec[i] = cs / (cabs(cs) + DBL_MIN);
        }
        break;
 
    default: /* SIMP: simple 1/N weighting */
        for (unsigned i = 0; i < num_bins; i++) {
            double complex cs = fftb[i] * conj(ffta[i]);
            xspec[i] = cs / (double)_N;
        }
        break;
    }
 
    /* Inverse FFT of the cross-spectrum -> time-domain cross-correlation */
    fftw_execute(px);
 
    /* Rearrange so the zero-lag is at the centre of the output array */
    _fftshift(xcorr, lagvals, _N);
}