InitialHEADmaster

1 files changed, 334 insertions, 0 deletions

diff --git a/lib/filter.cpp b/lib/filter.cpp
new file mode 100644
index 0000000..0ca829e
--- /dev/null
+++ b/lib/filter.cpp
@@ -0,0 +1,334 @@
+/* Copyright 2012 Dietrich Epp <depp@zdome.net> */
+#include "filter.h"
+#include "param.h"
+#include "../include/fresample.h"
+#include <cmath>
+#include <memory>
+#include <exception>
+#include <numeric>
+#include <algorithm>
+#include <iostream>
+
+namespace {
+/*
+  Simple sinc function implementation.  Note that this is the
+  unnormalized sinc function, with zeroes at nonzero multiples of pi.
+*/
+constexpr double sinc(double x)
+{
+	return std::abs(x) < 1e-8 ? 1.0 : std::sin(x) / x;
+}
+
+/*
+  Modified Bessel function of the first kind of order 0, i0.  Only
+  considered valid over the domain [0,18].
+
+  This is the naive algorithm.
+
+  In "Computation of Special Functions" by Shanjie Zhang and Jianming
+  Jin, this technique is only used for 0<X<18.  However, we only need
+  it for the Kaiser window, and a beta of 18 is fairly ridiculous --
+  it gives a side lobe height of -180 dB, which is not only beyond the
+  actual SNR of high end recording systems, it is beyond the SNR of
+  theoretically perfect 24-bit systems.
+*/
+constexpr double bessel_i0(double x)
+{
+    double a = 1.0, y = 1.0, x2 = x*x;
+    for (int i = 1; i <= 50; ++i) {
+        a *= 0.25 * x2 / (i * i);
+        y += a;
+        if (a < y * 1e-15)
+            break;
+    }
+    return y;
+}
+
+std::vector<double> filter_calculate(std::size_t nsamp, std::size_t nfilt, double offset, double cutoff, double beta)
+{
+	std::vector<double> data(nsamp * nfilt);
+
+	const auto yscale = 2.0 * cutoff / bessel_i0(beta);
+	const auto xscale = (8.0 * std::atan(1.0)) * cutoff;
+	for (std::size_t i = 0; i < nfilt; ++i) {
+		const auto x0 = (nsamp - 1) / 2 + offset * i;
+		for (std::size_t j = 0; j < nsamp; ++j) {
+			const auto x = j - x0;
+			const auto t = x * (2.0 / (nsamp - 2));
+			double y;
+			if (t <= -1.0 || t >= 1.0) {
+				y = 0.0;
+			} else {
+				y = yscale * bessel_i0(beta * sqrt(1.0 - t * t)) * sinc(xscale * x);
+			}
+			data[i * nsamp + j] = y;
+		}
+	}
+
+	return data;
+}
+
+template<typename type, type scale>
+void filter_calculate_integral(type* data, std::size_t nsamp, std::size_t nfilt, double offset, double cutoff, double beta)
+{
+	const auto& filter = filter_calculate(nsamp, nfilt, offset, cutoff, beta);
+
+	for (std::size_t i = 0; i < nfilt; ++i) {
+		const auto sum = accumulate(begin(filter) + i * nsamp, begin(filter) + (i + 1) * nsamp, 0.0);
+		const auto fac = static_cast<double>(scale) / sum;
+		auto err = 0.0;
+		for (std::size_t j = 0; j < nsamp; ++j) {
+			const auto y = filter[i * nsamp + j] * fac + err;
+			const auto z = std::round(y);
+			err = z - y;
+			data[i * nsamp + j] = static_cast<type>(z);
+		}
+	}
+}
+
+void lfr_filter_calculate_s16(std::int16_t* data, std::size_t nsamp, std::size_t nfilt, double offset, double cutoff, double beta)
+{
+	if(nsamp <= 8)
+	{
+		filter_calculate_integral<std::int16_t, 31500>(data, nsamp, nfilt, offset, cutoff, beta);
+	}
+	else
+	{
+		filter_calculate_integral<std::int16_t, 32767>(data, nsamp, nfilt, offset, cutoff, beta);
+	}
+}
+
+void lfr_filter_calculate_f32(float *data, std::size_t nsamp, std::size_t nfilt, double offset, double cutoff, double beta)
+{
+	const auto& filter = filter_calculate(nsamp, nfilt, offset, cutoff, beta);
+
+	transform(begin(filter), end(filter), data, [](const auto val){ return static_cast<float>(val); });
+}
+}
+
+lfr_filter::lfr_filter(lfr_param& param)
+{
+    double dw, dw2, lenf, atten2, atten3, maxerror, beta;
+    double ierror, rerror, error;
+    double t, a, ulp;
+    int align=8, max_oversample;
+
+    param.calculate();
+
+    f_pass = param.param[LFR_PARAM_FPASS];
+    f_stop = param.param[LFR_PARAM_FSTOP];
+    atten  = param.param[LFR_PARAM_ATTEN];
+
+    /* atten2 is the attenuation of the ideal (un-rounded) filter,
+       maxerror is the ratio of a 0 dBFS signal to the permitted
+       roundoff and interpolation error.  This divides the
+       "attenuation" into error budgets with the assumption that the
+       error from each source is uncorrelated.  Note that atten2 is
+       measured in dB, whereas maxerror is a ratio to a full scale
+       signal.  */
+    atten2 = atten + 3.0;
+    maxerror = exp((atten + 3.0) * (-log(10.0) / 20.0));
+
+    /* Calculate the filter order from the attenuation.  Round up to
+       the nearest multiple of the SIMD register alignment.  */
+    dw = (8.0 * std::atan(1.0)) * (f_stop - f_pass);
+    lenf = (atten2 - 8.0) / (4.57 * dw) + 1;
+    nsamp = static_cast<int>(std::ceil(lenf / align) * align);
+    if (nsamp < align)
+        nsamp = align;
+
+    /* ========================================
+       Determine filter coefficient size.
+       ======================================== */
+
+    /* We can calculate expected roundoff error from filter length.
+       We assume that the roundoff error at each coefficient is an
+       independent uniform variable with a range of 1 ULP, and that
+       there is an equal roundoff step during interpolation.  This
+       gives roundoff error with a variance of N/6 ULP^2.  Roundoff
+       error is a ratio relative to 1 ULP.  */
+    rerror = std::sqrt(nsamp * (1.0 / 6.0));
+
+    /* The interpolation error can be adjusted by choosing the amount
+       of oversampling.  The curvature of the sinc function is bounded
+       by (2*pi*f)^2, so the interpolation error is bounded by
+       (pi*f/M)^2.
+
+       We set a maximum oversampling of 2^8 for 16-bit, and 2^12 for
+       floating point.  */
+    t = f_pass * (4.0 * std::atan(1.0));
+    a = 1.0 / 256;
+    ierror = (t * a) * (t * a);
+    ulp = 1.0 / 32768.0;
+    /* We assume, again, that interpolation and roundoff error are
+       uncorrelated and normal.  If error at 16-bit exceeds our
+       budget, then use floating point.  */
+    error = (ierror * ierror + rerror * rerror) * (ulp * ulp);
+    if (error <= maxerror * maxerror) {
+        type = LFR_FTYPE_S16;
+        ulp = 1.0 / 32768.0;
+        max_oversample = 8;
+    } else {
+        type = LFR_FTYPE_F32;
+        ulp = 1.0 / 16777216.0;
+        max_oversample = 12;
+    }
+
+    /* Calculate the interpolation error budget by subtracting
+       roundoff error from the total error budget, since roundoff
+       error is fixed.  */
+    ierror = maxerror * maxerror - (rerror * rerror) * (ulp * ulp);
+    ierror = (ierror > 0) ? sqrt(ierror) : 0;
+    if (ierror < ulp)
+        ierror = ulp;
+
+    /* Calculate oversampling from the error budget.  */
+    a = (f_pass * (4.0 * std::atan(1.0))) / std::sqrt(ierror);
+    log2nfilt = static_cast<int>(std::ceil(std::log(a) * (1.0 / std::log(2.0))));
+    if (log2nfilt < 0)
+        log2nfilt = 0;
+    else if (log2nfilt > max_oversample)
+        log2nfilt = max_oversample;
+
+    /* ========================================
+       Calculate window parameters
+       ======================================== */
+
+    /* Since we rounded up the filter order, we can increase the stop
+       band attenuation or bandwidth without making the transition
+       bandwidth exceed the design parameters.  This gives a "free"
+       boost in quality.  We choose to increase both.
+
+       If we didn't increase these parameters, we would still incur
+       the additional computational cost but it would be spent
+       increasing the width of the stop band, which is not useful.  */
+
+    /* atten3 is the free stopband attenuation */
+    atten3 = (nsamp - 1) * 4.57 * dw + 8;
+    t = (-20.0 / std::log(10.0)) * std::log(rerror * ulp);
+    if (t < atten3)
+        atten3 = t;
+    if (atten2 > atten3)
+        atten3 = atten2;
+    else
+        atten3 = 0.5 * (atten2 + atten3);
+
+    /* f_pass2 is the free pass band */
+    dw2 = (atten3 - 8.0) / (4.57 * (nsamp - 1));
+    auto f_pass2 = f_stop - dw2 * (1.0 / (8.0 * std::atan(1.0)));
+
+    if (atten3 > 50)
+        beta = 0.1102 * (atten3 - 8.7);
+    else if (atten3 > 21)
+      beta = 0.5842 * std::pow(atten3 - 21, 0.4) + 0.07866 * (atten3 - 21);
+    else
+        beta = 0;
+
+    setup_window(f_pass2, beta);
+}
+
+void lfr_filter::setup_window(double cutoff, double beta)
+{
+    size_t esz = 1, align = 16, nfilt;
+    if (nsamp < 1 || log2nfilt < 0)
+        goto error;
+
+    switch (type) {
+    case LFR_FTYPE_S16:
+        esz = sizeof(short);
+        break;
+
+    case LFR_FTYPE_F32:
+        esz = sizeof(float);
+        break;
+    }
+    nfilt = (1u << log2nfilt) + 1;
+    if ((size_t) nsamp > (size_t) -1 / esz)
+        goto error;
+    if (((size_t) -1 - (align - 1) - sizeof(*this)) / nfilt < nsamp * esz)
+        goto error;
+    data.resize(nsamp * nfilt * esz + align - 1);
+
+    delay = static_cast<lfr_fixed_t>((nsamp - 1) / 2) << 32;
+
+    switch (type) {
+    case LFR_FTYPE_S16:
+        lfr_filter_calculate_s16(
+            reinterpret_cast<short int*>(data.data()), nsamp, nfilt,
+            1.0 / static_cast<double>(1 << log2nfilt), cutoff, beta);
+        break;
+
+    case LFR_FTYPE_F32:
+        lfr_filter_calculate_f32(
+            reinterpret_cast<float*>(data.data()), nsamp, nfilt,
+            1.0 / static_cast<double>(1 << log2nfilt), cutoff, beta);
+        break;
+    }
+    return;
+
+error:
+    throw std::runtime_error{"Error in filter setup: lfr_filter::setup_window"};
+}
+
+int lfr_filter::geti(LFR_INFO_TYPE iname, bool convert) const
+{
+	switch(iname)
+	{
+		case LFR_INFO_DELAY:
+			return delay * (1.0 / 4294967296.0);
+		case LFR_INFO_FPASS:
+			return f_pass;
+		case LFR_INFO_FSTOP:
+			return f_stop;
+		case LFR_INFO_ATTEN:
+			return atten;
+		default:
+			if(convert)
+			{
+				return static_cast<int>(getf(iname, false));
+			}
+	}
+	return {};
+}
+
+double lfr_filter::getf(LFR_INFO_TYPE iname, bool convert) const
+{
+	switch(iname)
+	{
+		case LFR_INFO_DELAY:
+			return static_cast<int>(delay >> 32);
+		case LFR_INFO_SIZE:
+			return nsamp;
+		case LFR_INFO_MEMSIZE:
+		{
+			int sz = nsamp * ((1 << log2nfilt) + 1);
+			switch (type)
+			{
+				case LFR_FTYPE_S16: return sz * 2;
+				case LFR_FTYPE_F32: return sz * 4;
+				default: return {};
+			}
+		}
+				default:
+					if(convert)
+					{
+						return geti(iname, false);
+					}
+	}
+	return {};
+}
+
+void lfr_filter::resample(
+	lfr_fixed_t *pos, lfr_fixed_t inv_ratio,
+	unsigned *dither, int nchan,
+	void *out, lfr_fmt_t outfmt, int outlen,
+	const void *in, lfr_fmt_t infmt, int inlen)
+{
+	lfr_resample_func_t func;
+	if (outfmt == LFR_FMT_S16_NATIVE && infmt == LFR_FMT_S16_NATIVE) {
+		func = lfr_resample_s16func(nchan, this);
+		if (!func)
+			return;
+		func(pos, inv_ratio, dither, out, outlen, in, inlen, this);
+	}
+}
\ No newline at end of file