minidsp_core.c

Go to the documentation of this file.

 
#include "minidsp.h"
#include "minidsp_internal.h"
 
/* -----------------------------------------------------------------------
 * 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)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
    if (N == 1) return a[0] * a[0];
    return MD_dot(a, a, N);
}

double MD_power(const double *a, unsigned N)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
    MD_CHECK(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0", 0.0);
    return MD_energy(a, N) / (double)N;
}

double MD_power_db(const double *a, unsigned N)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
    MD_CHECK(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0", 0.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)
{
    MD_CHECK_VOID(in != NULL, MD_ERR_NULL_POINTER, "in is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(oldmin < oldmax, MD_ERR_INVALID_RANGE, "oldmin must be < oldmax");
    MD_CHECK_VOID(newmin < newmax, MD_ERR_INVALID_RANGE, "newmin must be < 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)
{
    MD_CHECK_VOID(in != NULL, MD_ERR_NULL_POINTER, "in is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(newmin < newmax, MD_ERR_INVALID_RANGE, "newmin must be < 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);
 
    /* Guard: cannot amplify silence — copy input and return */
    if (input_energy == 0.0) {
        for (unsigned i = 0; i < N; i++) out[i] = in[i];
        return;
    }
 
    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)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
 
    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: signal analysis
 * -----------------------------------------------------------------------*/

#define MD_F0_ACF_PEAK_THRESHOLD 0.15

static double md_parabolic_offset(double y_left, double y_mid, double y_right)
{
    double denom = y_left - 2.0 * y_mid + y_right;
    if (fabs(denom) < 1e-12) return 0.0;
 
    double delta = 0.5 * (y_left - y_right) / denom;
    if (delta < -0.5) delta = -0.5;
    if (delta >  0.5) delta =  0.5;
    return delta;
}

double MD_rms(const double *a, unsigned N)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
    MD_CHECK(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0", 0.0);
    return sqrt(MD_power(a, N));
}

double MD_zero_crossing_rate(const double *a, unsigned N)
{
    MD_CHECK(a != NULL, MD_ERR_NULL_POINTER, "a is NULL", 0.0);
    MD_CHECK(N > 1, MD_ERR_INVALID_SIZE, "N must be > 1", 0.0);
    unsigned crossings = 0;
    for (unsigned i = 1; i < N; i++) {
        if ((a[i] < 0.0) != (a[i - 1] < 0.0))
            crossings++;
    }
    return (double)crossings / (double)(N - 1);
}

void MD_autocorrelation(const double *a, unsigned N,
                        double *out, unsigned max_lag)
{
    MD_CHECK_VOID(a != NULL, MD_ERR_NULL_POINTER, "a is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0");
    MD_CHECK_VOID(max_lag > 0 && max_lag < N, MD_ERR_INVALID_RANGE, "max_lag must be in (0, N)");
 
    double r0 = MD_energy(a, N);
    if (r0 == 0.0) {
        memset(out, 0, max_lag * sizeof(double));
        return;
    }
    for (unsigned tau = 0; tau < max_lag; tau++) {
        out[tau] = MD_dot(a, a + tau, N - tau) / r0;
    }
}

void MD_peak_detect(const double *a, unsigned N, double threshold,
                    unsigned min_distance, unsigned *peaks_out,
                    unsigned *num_peaks_out)
{
    MD_CHECK_VOID(a != NULL, MD_ERR_NULL_POINTER, "a is NULL");
    MD_CHECK_VOID(peaks_out != NULL, MD_ERR_NULL_POINTER, "peaks_out is NULL");
    MD_CHECK_VOID(num_peaks_out != NULL, MD_ERR_NULL_POINTER, "num_peaks_out is NULL");
    MD_CHECK_VOID(min_distance >= 1, MD_ERR_INVALID_SIZE, "min_distance must be >= 1");
 
    unsigned count = 0;
    unsigned last_peak = 0;
 
    for (unsigned i = 1; i + 1 < N; i++) {
        if (a[i] > a[i - 1] && a[i] > a[i + 1] && a[i] >= threshold) {
            if (count == 0 || i - last_peak >= min_distance) {
                peaks_out[count++] = i;
                last_peak = i;
            }
        }
    }
    *num_peaks_out = count;
}
 
double MD_f0_autocorrelation(const double *signal, unsigned N,
                             double sample_rate,
                             double min_freq_hz, double max_freq_hz)
{
    MD_CHECK(signal != NULL, MD_ERR_NULL_POINTER, "signal is NULL", 0.0);
    MD_CHECK(N >= 2, MD_ERR_INVALID_SIZE, "N must be >= 2", 0.0);
    MD_CHECK(sample_rate > 0.0, MD_ERR_INVALID_RANGE, "sample_rate must be > 0", 0.0);
    MD_CHECK(min_freq_hz > 0.0, MD_ERR_INVALID_RANGE, "min_freq_hz must be > 0", 0.0);
    MD_CHECK(max_freq_hz > min_freq_hz, MD_ERR_INVALID_RANGE, "max_freq_hz must be > min_freq_hz", 0.0);
 
    unsigned lag_min = (unsigned)floor(sample_rate / max_freq_hz);
    unsigned lag_max = (unsigned)ceil(sample_rate / min_freq_hz);
 
    if (lag_min < 1) lag_min = 1;
    if (lag_max > N - 2) lag_max = N - 2;
    if (lag_min > lag_max) return 0.0;
    if (lag_max < 2) return 0.0;  /* need lag-1 and lag+1 neighbors */
 
    double *acf = malloc((lag_max + 1) * sizeof(double));
    if (!acf) return 0.0;
 
    MD_autocorrelation(signal, N, acf, lag_max + 1);
 
    unsigned start = (lag_min < 1) ? 1 : lag_min;
    unsigned stop  = (lag_max > N - 2) ? (N - 2) : lag_max;
 
    double best_peak = -DBL_MAX;
    unsigned best_lag = 0;
 
    for (unsigned lag = start; lag <= stop; lag++) {
        double y = acf[lag];
        if (y < MD_F0_ACF_PEAK_THRESHOLD) continue;
        if (y <= acf[lag - 1] || y <= acf[lag + 1]) continue;
 
        if (y > best_peak) {
            best_peak = y;
            best_lag = lag;
        }
    }
 
    if (best_lag == 0) {
        free(acf);
        return 0.0;
    }
 
    double lag_est = (double)best_lag;
    if (best_lag > 0 && best_lag + 1 <= lag_max) {
        lag_est += md_parabolic_offset(acf[best_lag - 1],
                                       acf[best_lag],
                                       acf[best_lag + 1]);
    }
    free(acf);
 
    if (lag_est <= 0.0) return 0.0;
    return sample_rate / lag_est;
}

void MD_mix(const double *a, const double *b, double *out,
            unsigned N, double w_a, double w_b)
{
    MD_CHECK_VOID(a != NULL, MD_ERR_NULL_POINTER, "a is NULL");
    MD_CHECK_VOID(b != NULL, MD_ERR_NULL_POINTER, "b is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    for (unsigned i = 0; i < N; i++) {
        out[i] = w_a * a[i] + w_b * b[i];
    }
}
 
/* -----------------------------------------------------------------------
 * Public API: simple effects
 * -----------------------------------------------------------------------*/
 
void MD_delay_echo(const double *in, double *out, unsigned N,
                   unsigned delay_samples, double feedback,
                   double dry, double wet)
{
    MD_CHECK_VOID(in != NULL, MD_ERR_NULL_POINTER, "in is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0");
    MD_CHECK_VOID(delay_samples > 0, MD_ERR_INVALID_SIZE, "delay_samples must be > 0");
    MD_CHECK_VOID(fabs(feedback) < 1.0, MD_ERR_INVALID_RANGE, "feedback must be in (-1, 1)");
 
    double *delay = calloc(delay_samples, sizeof(double));
    MD_CHECK_VOID(delay != NULL, MD_ERR_ALLOC_FAILED, "calloc failed");
 
    unsigned idx = 0;
    for (unsigned n = 0; n < N; n++) {
        double x = in[n];
        double d = delay[idx];
        out[n] = dry * x + wet * d;
        delay[idx] = x + feedback * d;
        idx++;
        if (idx == delay_samples) idx = 0;
    }
 
    free(delay);
}
 
void MD_tremolo(const double *in, double *out, unsigned N,
                double rate_hz, double depth, double sample_rate)
{
    MD_CHECK_VOID(in != NULL, MD_ERR_NULL_POINTER, "in is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0");
    MD_CHECK_VOID(sample_rate > 0.0, MD_ERR_INVALID_RANGE, "sample_rate must be > 0");
    MD_CHECK_VOID(rate_hz >= 0.0, MD_ERR_INVALID_RANGE, "rate_hz must be >= 0");
    MD_CHECK_VOID(depth >= 0.0 && depth <= 1.0, MD_ERR_INVALID_RANGE, "depth must be in [0, 1]");
 
    double phase_step = 2.0 * M_PI * rate_hz / sample_rate;
    for (unsigned n = 0; n < N; n++) {
        double lfo = 0.5 * (1.0 + sin(phase_step * (double)n));
        double gain = (1.0 - depth) + depth * lfo;
        out[n] = in[n] * gain;
    }
}
 
void MD_comb_reverb(const double *in, double *out, unsigned N,
                    unsigned delay_samples, double feedback,
                    double dry, double wet)
{
    MD_CHECK_VOID(in != NULL, MD_ERR_NULL_POINTER, "in is NULL");
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(N > 0, MD_ERR_INVALID_SIZE, "N must be > 0");
    MD_CHECK_VOID(delay_samples > 0, MD_ERR_INVALID_SIZE, "delay_samples must be > 0");
    MD_CHECK_VOID(fabs(feedback) < 1.0, MD_ERR_INVALID_RANGE, "feedback must be in (-1, 1)");
 
    double *comb = calloc(delay_samples, sizeof(double));
    MD_CHECK_VOID(comb != NULL, MD_ERR_ALLOC_FAILED, "calloc failed");
 
    unsigned idx = 0;
    for (unsigned n = 0; n < N; n++) {
        double x = in[n];
        double delayed = comb[idx];
        double y_comb = x + feedback * delayed;
        comb[idx] = y_comb;
        out[n] = dry * x + wet * y_comb;
        idx++;
        if (idx == delay_samples) idx = 0;
    }
 
    free(comb);
}
 
/* -----------------------------------------------------------------------
 * Public API: math utilities
 * -----------------------------------------------------------------------*/

double MD_bessel_i0(double x)
{
    double sum  = 1.0;
    double term = 1.0;
    double half_x = x / 2.0;
 
    for (unsigned k = 1; k < 300; k++) {
        term *= (half_x / (double)k);
        double term_sq = term * term;
        sum += term_sq;
        if (term_sq < 1e-15 * sum) break;
    }
    return sum;
}

double MD_sinc(double x)
{
    if (fabs(x) < 1e-12) return 1.0;
    double px = M_PI * x;
    return sin(px) / px;
}
 
/* -----------------------------------------------------------------------
 * Public API: window generation
 * -----------------------------------------------------------------------*/

void MD_Gen_Hann_Win(double *out, unsigned n)
{
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(n > 0, MD_ERR_INVALID_SIZE, "n must be > 0");
 
    if (n == 1) {
        out[0] = 1.0;
        return;
    }
 
    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));
    }
}

void MD_Gen_Hamming_Win(double *out, unsigned n)
{
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(n > 0, MD_ERR_INVALID_SIZE, "n must be > 0");
 
    if (n == 1) {
        out[0] = 1.0;
        return;
    }
 
    double n_minus_1 = (double)(n - 1);
    for (unsigned i = 0; i < n; i++) {
        out[i] = 0.54 - 0.46 * cos(2.0 * M_PI * (double)i / n_minus_1);
    }
}

void MD_Gen_Blackman_Win(double *out, unsigned n)
{
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(n > 0, MD_ERR_INVALID_SIZE, "n must be > 0");
 
    if (n == 1) {
        out[0] = 1.0;
        return;
    }
 
    double n_minus_1 = (double)(n - 1);
    for (unsigned i = 0; i < n; i++) {
        double p = 2.0 * M_PI * (double)i / n_minus_1;
        out[i] = 0.42 - 0.5 * cos(p) + 0.08 * cos(2.0 * p);
    }
}

void MD_Gen_Rect_Win(double *out, unsigned n)
{
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(n > 0, MD_ERR_INVALID_SIZE, "n must be > 0");
    for (unsigned i = 0; i < n; i++) {
        out[i] = 1.0;
    }
}

void MD_Gen_Kaiser_Win(double *out, unsigned n, double beta)
{
    MD_CHECK_VOID(out != NULL, MD_ERR_NULL_POINTER, "out is NULL");
    MD_CHECK_VOID(n > 0, MD_ERR_INVALID_SIZE, "n must be > 0");
 
    if (n == 1) {
        out[0] = 1.0;
        return;
    }
 
    double inv_i0_beta = 1.0 / MD_bessel_i0(beta);
    double n_minus_1 = (double)(n - 1);
 
    for (unsigned i = 0; i < n; i++) {
        double t = 2.0 * (double)i / n_minus_1 - 1.0;  /* in [-1, 1] */
        double arg = 1.0 - t * t;
        if (arg < 0.0) arg = 0.0;  /* clamp floating-point rounding */
        out[i] = MD_bessel_i0(beta * sqrt(arg)) * inv_i0_beta;
    }
}