SincResampler.java

/**
 * This file is part of reSID, a MOS6581 SID emulator engine. Copyright (C) 2004  Dag Lem <resid@nimrod.no>
 * <p>
 * This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public
 * License as published by the Free Software Foundation; either version 2 of the License or (at your option) any later
 * version.
 * <p>
 * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 * details.
 * <p>
 * You should have received a copy of the GNU General Public License along with this program; if not, write to the Free
 * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * @author Ken Händel
 *
 */
package builder.resid.resample;

import java.util.Arrays;
import java.util.HashMap;
import java.util.Map;
import java.util.Map.Entry;
import java.util.TreeMap;

/**
 * This is the theoretically correct (and computationally intensive) audio sample generation. The samples are generated
 * by resampling to the specified sampling frequency. The work rate is inversely proportional to the percentage of the
 * bandwidth allocated to the filter transition band.
 * <p>
 * This implementation is based on the paper "A Flexible Sampling-Rate Conversion Method", by J. O. Smith and P. Gosset,
 * or rather on the expanded tutorial on the "Digital Audio Resampling Home Page":
 * http:*www-ccrma.stanford.edu/~jos/resample/
 * <p>
 * By building shifted FIR tables with samples according to the sampling frequency, this implementation dramatically
 * reduces the computational effort in the filter convolutions, without any loss of accuracy. The filter convolutions
 * are also vectorizable on current hardware.
 * <p>
 * Further possible optimizations are:
 * <OL>
 * <LI>An equiripple filter design could yield a lower filter order, see
 * <a href="http://www.mwrf.com/Articles/ArticleID/7229/7229.html">here</a>
 * <LI>The Convolution Theorem could be used to bring the complexity of
 * convolution down from O(n*n) to O(n*log(n)) using the Fast Fourier Transform, see
 * http://en.wikipedia.org/wiki/Convolution_theorem
 * <LI>Simply resampling in two steps can also yield computational savings,
 * since the transition band will be wider in the first step and the required filter order is thus lower in this step.
 * Laurent Ganier has found the optimal intermediate sampling frequency to be (via derivation of sum of two steps):
 * <BR>
 * <CODE>2 * pass_freq + sqrt [ 2 * pass_freq * orig_sample_freq
 * * (dest_sample_freq - 2 * pass_freq) / dest_sample_freq ]</CODE>
 * </OL>
 *
 * @author Dag Lem
 * @author Antti Lankila
 */
public final class SincResampler implements StereoResampler {
    private static final int RINGSIZE = 2048;

    private static final int BITS = 16;

    private final int[] sample = new int[RINGSIZE * 2];
    private final int[] sampleR = new int[RINGSIZE * 2];

    private int[][] fir;

    private int sampleIndex;

    private final int firRES;
    private int firN;

    // Retain the fractional period so SID and cartridge sample clocks do not drift.
    private final double cyclesPerSample;

    private double sampleOffset;

    private int output;
    private int outputR;

    /**
     * Cache for caching the expensive FIR table computation results in the Java process.
     */
    private static final Map<String, int[][]> FIR_CACHE = new HashMap<>();

    /**
     * Maximum error acceptable in I0 is 1e-6, or ~96 dB.
     */
    private static final double I0E = 1e-6;

    /**
     * I0() computes the 0th order modified Bessel function of the first kind. This function is originally from
     * resample-1.5/filterkit.c by J. O. Smith. It is used to build the Kaiser window for resampling.
     *
     * @param x evaluate I0 at x
     * @return value of I0 at x.
     */
    private static double I0(final double x) {
        double sum = 1, u = 1, n = 1;
        final double halfx = x / 2;

        do {
            final double temp = halfx / n;
            u *= temp * temp;
            sum += u;
            n += 1;
        } while (u >= I0E * sum);

        return sum;
    }

    /**
     * Calculate convolution with sample and sinc.
     *
     * @param a    sample buffer input
     * @param aPos offset in sample buffer
     * @param b    sinc
     * @return convolved result
     */
    private static int convolve(final int[] a, int aPos, final int[] b) {
        int out = 0;
        for (int i = 0; i < b.length; i++, aPos++) {
            out += a[aPos] * b[i];
        }
        return out + (1 << 14) >> 15;
    }

    private long convolveStereo(int sampleStart, int[] coefficients) {
        int outL = 0;
        int outR = 0;
        for (int i = 0; i < coefficients.length; i++, sampleStart++) {
            int coefficient = coefficients[i];
            outL += sample[sampleStart] * coefficient;
            outR += sampleR[sampleStart] * coefficient;
        }
        outL = outL + (1 << 14) >> 15;
        outR = outR + (1 << 14) >> 15;
        return (long) outL << 32 | outR & 0xffffffffL;
    }

    /**
     * Use a clock frequency of 985248Hz for PAL C64, 1022730Hz for NTSC C64. The default end of passband frequency is
     * pass_freq = 0.9*sample_freq/2 for sample frequencies up to ~ 44.1kHz, and 20kHz for higher sample frequencies.
     * <p>
     * For resampling, the ratio between the clock frequency and the sample frequency is limited as follows:
     * 125*clock_freq/sample_freq &lt; 16384 E.g., provided a clock frequency of ~ 1MHz, the sample frequency cannot be
     * set lower than ~ 8kHz. A lower sample frequency would make the resampling code overfill its 16k sample ring
     * buffer.
     * <p>
     * The end of passband frequency is also limited: pass_freq &lt;= 0.9*sample_freq/2
     * <p>
     * E.g., for a 44.1kHz sampling rate, the end of passband frequency is limited to slightly below 20kHz. This
     * constraint ensures that the FIR table is not overfilled.
     *
     * @param clockFrequency           System clock frequency at Hz
     * @param samplingFrequency        Desired output sampling rate
     * @param highestAccurateFrequency highest accurate frequency
     */
    public SincResampler(final double clockFrequency, final double samplingFrequency,
                         final double highestAccurateFrequency) {
        cyclesPerSample = clockFrequency / samplingFrequency * 1024.0;

        // 16 bits -> -96dB stopband attenuation.
        final double A = -20 * Math.log10(1.0 / (1 << BITS));
        // A fraction of the bandwidth is allocated to the transition band,
        // which we double
        // because we design the filter to transition halfway at nyquist.
        final double dw = (1 - 2 * highestAccurateFrequency / samplingFrequency) * Math.PI * 2;

        // For calculation of beta and N see the reference for the kaiserord
        // function in the MATLAB Signal Processing Toolbox:
        // http://www.mathworks.com/access/helpdesk/help/toolbox/signal/kaiserord.html
        final double beta = 0.1102 * (A - 8.7);
        final double I0beta = I0(beta);
        final double cyclesPerSampleD = clockFrequency / samplingFrequency;

        {
            // The filter order will maximally be 124 with the current
            // constraints.
            // N >= (96.33 - 7.95)/(2 * pi * 2.285 * (maxfreq - passbandfreq) >=
            // 123
            // The filter order is equal to the number of zero crossings, i.e.,
            // it should be an even number (sinc is symmetric about x = 0).
            int N = (int) ((A - 7.95) / (2.285 * dw) + 0.5);
            N += N & 1;

            // The filter length is equal to the filter order + 1.
            // The filter length must be an odd number (sinc is symmetric about
            // x = 0).
            firN = (int) (N * cyclesPerSampleD) + 1;
            firN |= 1;

            // Check whether the sample ring buffer would overflow.
            if (firN > RINGSIZE - 1) {
                throw new RuntimeException(
                        String.format("Resampling quality exceeds the available buffer: %d required, but only has %d",
                                firN, RINGSIZE));
            }

            /*
             * Error is bounded by err < 1.234 / L^2, so L = sqrt(1.234 / (2^-16)) =
             * sqrt(1.234 * 2^16).
             */
            firRES = (int) Math.ceil(Math.sqrt(1.234 * (1 << BITS)) / cyclesPerSampleD);

            /*
             * firN*firRES represent the total resolution of the sinc sampling. JOS
             * recommends a length of 2^BITS, but we don't quite use that good a filter. The
             * filter test program indicates that the filter performs well, though.
             */
        }

        // The cutoff frequency is midway through the transition band, in effect
        // the same as nyquist.
        final double wc = Math.PI;

        final String firKey = firN + "," + firRES + "," + cyclesPerSampleD;
        fir = FIR_CACHE.get(firKey);

        /*
         * The FIR computation is expensive, and we set sampling parameters often, but
         * from a very small set of choices. Thus, caching is used to speed
         * initialization.
         */
        if (fir == null) {
            // Allocate memory for FIR tables.
            fir = new int[firRES][];
            for (int i = 0; i < firRES; i++) {
                fir[i] = new int[firN];
            }
            FIR_CACHE.put(firKey, fir);

            /* Calculate the sinc tables. */
            final double scale = 32768.0 * wc / cyclesPerSampleD / Math.PI;
            for (int i = 0; i < firRES; i++) {
                final double jPhase = (double) i / firRES + (double) firN / 2;
                for (int j = 0; j < firN; j++) {
                    final double x = j - jPhase;

                    final double xt = x / ((double) firN / 2);
                    final double kaiserXt = Math.abs(xt) < 1 ? I0(beta * Math.sqrt(1 - xt * xt)) / I0beta : 0;

                    final double wt = wc * x / cyclesPerSampleD;
                    final double sincWt = Math.abs(wt) >= 1e-8 ? Math.sin(wt) / wt : 1;

                    fir[i][j] = (int) (scale * sincWt * kaiserXt);
                }
            }
        }
    }

    /**
     * Inputs a given sample into this SincResampler.
     *
     * @param input The sample to input into this resampler.
     * @return True if the sample is ready to output.
     */
    @Override
    public boolean input(int input) {
        boolean ready = false;

        sample[sampleIndex] = sample[sampleIndex + RINGSIZE] = input;
        sampleIndex = sampleIndex + 1 & RINGSIZE - 1;

        if (sampleOffset < 1024) {
            output = fir((int) sampleOffset);
            ready = true;
            sampleOffset += cyclesPerSample;
        }
        sampleOffset -= 1024;

        return ready;
    }

    @Override
    public boolean input(int inputL, int inputR) {
        boolean ready = false;

        sample[sampleIndex] = sample[sampleIndex + RINGSIZE] = inputL;
        sampleR[sampleIndex] = sampleR[sampleIndex + RINGSIZE] = inputR;
        sampleIndex = sampleIndex + 1 & RINGSIZE - 1;

        if (sampleOffset < 1024) {
            firStereo((int) sampleOffset);
            ready = true;
            sampleOffset += cyclesPerSample;
        }
        sampleOffset -= 1024;
        return ready;
    }

    /**
     * Gets the current output sample.
     *
     * @return The current output sample.
     */
    @Override
    public int output() {
        return output;
    }

    @Override
    public int outputL() {
        return output;
    }

    @Override
    public int outputR() {
        return outputR;
    }

    /**
     * Resets this SincResampler.
     */
    @Override
    public void reset() {
        Arrays.fill(sample, 0);
        Arrays.fill(sampleR, 0);
        // The first output is due after one sample period, not at time zero.
        sampleOffset = cyclesPerSample;
    }

    private int fir(int subcycle) {
        /* find the first of the nearest fir tables close to the phase */
        int firTableFirst = subcycle * firRES >> 10;
        int firTableOffset = subcycle * firRES & 0x3ff;

        /*
         * find firN most recent samples, plus one extra in case the FIR wraps.
         */
        int sampleStart = sampleIndex - firN + RINGSIZE - 1;

        final int v1 = convolve(sample, sampleStart, fir[firTableFirst]);
        // Use the next FIR table, wrap around to the first FIR table using
        // the sample.
        if (++firTableFirst == firRES) {
            firTableFirst = 0;
            ++sampleStart;
        }

        final int v2 = convolve(sample, sampleStart, fir[firTableFirst]);

        // Linear interpolation between the sinc tables yields a good
        // approximation for the exact value.
        return v1 + (firTableOffset * (v2 - v1) >> 10);
    }

    private void firStereo(int subcycle) {
        int firTableFirst = subcycle * firRES >> 10;
        int firTableOffset = subcycle * firRES & 0x3ff;

        /*
         * find firN most recent samples, plus one extra in case the FIR wraps.
         */
        int sampleStart = sampleIndex - firN + RINGSIZE - 1;

        long v1 = convolveStereo(sampleStart, fir[firTableFirst]);
        // Use the next FIR table, wrap around to the first FIR table using
        // the sample.
        if (++firTableFirst == firRES) {
            firTableFirst = 0;
            ++sampleStart;
        }

        long v2 = convolveStereo(sampleStart, fir[firTableFirst]);

        // Linear interpolation between the sinc tables yields a good
        // approximation for the exact value.
        int v1L = (int) (v1 >> 32);
        int v1R = (int) v1;
        int v2L = (int) (v2 >> 32);
        int v2R = (int) v2;

        output = v1L + (firTableOffset * (v2L - v1L) >> 10);
        outputR = v1R + (firTableOffset * (v2R - v1R) >> 10);
    }

    /**
     * Simple sin waveform in, power output measurement function. It would be far better to use FFT.
     *
     * @param args Arguments
     */
    public static void main(String[] args) {
        double RATE = 985248.4;
        Resampler r = new TwoPassSincResampler(RATE, 48000.0, 20000.0);

        Map<Double, Double> results = new TreeMap<>();
        long start = System.currentTimeMillis();
        for (double freq = 1000; freq < RATE / 2; freq *= 1.01) {
            /* prefill resampler buffer */
            int k = 0;
            double omega = 2 * Math.PI * freq / RATE;
            for (int j = 0; j < RINGSIZE; j++) {
                int signal = (int) (32768.0 * Math.sin(k++ * omega) * Math.sqrt(2));
                r.input(signal);
            }

            int n = 0;
            float pwr = 0;
            /*
             * Now, during the measurement stage, put 100 cycles of waveform through a filter.
             */
            for (int j = 0; j < 100000; j++) {
                int signal = (int) (32768.0 * Math.sin(k++ * omega) * Math.sqrt(2));
                if (r.input(signal)) {
                    float out = r.output();
                    pwr += out * out;
                    n += 1;
                }
            }

            results.put(freq, 10 * Math.log10(pwr / n));
        }
        long end = System.currentTimeMillis();

        for (Entry<Double, Double> freq : results.entrySet()) {
            System.out.printf("%6.0f Hz %f dB%n", freq.getKey(), freq.getValue());
        }
        System.out.println("Filtering time " + (end - start) + " ms");
    }
}