From cbd117eb3a9eba367e58b9139f46f29294fe7ebe Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Wed, 10 Apr 2024 23:54:40 -0700
Subject: [PATCH] Util: Start bringing up better audio resampling

---
 include/mgba-util/interpolator.h | 33 +++++++++++++
 src/util/CMakeLists.txt          |  1 +
 src/util/interpolator.c          | 83 ++++++++++++++++++++++++++++++++
 3 files changed, 117 insertions(+)
 create mode 100644 include/mgba-util/interpolator.h
 create mode 100644 src/util/interpolator.c

diff --git a/include/mgba-util/interpolator.h b/include/mgba-util/interpolator.h
new file mode 100644
index 000000000..4cb7231fd
--- /dev/null
+++ b/include/mgba-util/interpolator.h
@@ -0,0 +1,33 @@
+/* Copyright (c) 2013-2024 Jeffrey Pfau
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+#ifndef M_INTERPOLATOR_H
+#define M_INTERPOLATOR_H
+
+#include <mgba-util/common.h>
+
+struct mSampleBuffer {
+	int16_t* data;
+	size_t samples;
+	int channels;
+};
+
+struct mInterpolator {
+	int16_t (*interpolate)(const struct mInterpolator* interp, const struct mSampleBuffer* data, double time, double sampleStep);
+};
+
+struct mInterpolatorSinc {
+	struct mInterpolator d;
+
+	unsigned resolution;
+	unsigned width;
+	double* sincLut;
+	double* windowLut;
+};
+
+void mInterpolatorSincInit(struct mInterpolatorSinc* interp, unsigned resolution, unsigned width);
+void mInterpolatorSincDeinit(struct mInterpolatorSinc* interp);
+
+#endif
diff --git a/src/util/CMakeLists.txt b/src/util/CMakeLists.txt
index eb82215d7..820a178d5 100644
--- a/src/util/CMakeLists.txt
+++ b/src/util/CMakeLists.txt
@@ -19,6 +19,7 @@ set(SOURCE_FILES
 	image.c
 	image/export.c
 	image/png-io.c
+	interpolator.c
 	patch.c
 	patch-fast.c
 	patch-ips.c
diff --git a/src/util/interpolator.c b/src/util/interpolator.c
new file mode 100644
index 000000000..2fceee898
--- /dev/null
+++ b/src/util/interpolator.c
@@ -0,0 +1,83 @@
+/* Copyright (c) 2013-2024 Jeffrey Pfau
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
+#include <mgba-util/interpolator.h>
+
+enum {
+	mSINC_RESOLUTION = 8192,
+	mSINC_WIDTH = 8,
+};
+
+static int16_t mInterpolatorSincInterpolate(const struct mInterpolator*, const struct mSampleBuffer* data, double time, double sampleStep);
+
+void mInterpolatorSincInit(struct mInterpolatorSinc* interp, unsigned resolution, unsigned width) {
+	interp->d.interpolate = mInterpolatorSincInterpolate;
+
+	if (!resolution) {
+		resolution = mSINC_RESOLUTION;
+	}
+	if (!width) {
+		width = mSINC_WIDTH;
+	}
+	unsigned samples = resolution * width;
+	double dy = M_PI / samples;
+	double y = dy;
+	double dx = dy * width;
+	double x = dx;
+
+	interp->sincLut = calloc(samples + 1, sizeof(double));
+	interp->windowLut = calloc(samples + 1, sizeof(double));
+
+	interp->sincLut[0] = 0;
+	interp->windowLut[0] = 1;
+
+	unsigned i;
+	for (i = 1; i <= samples; ++i, x += dx, y += dy) {
+		interp->sincLut[i] = x < width ? sin(x) / x : 0.0;
+		// Three term Nuttall window with continuous first derivative
+		interp->windowLut[i] = 0.40897 + 0.5 * cos(y) + 0.09103 * cos(2 * y);
+	}
+}
+
+void mInterpolatorSincDeinit(struct mInterpolatorSinc* interp) {
+	free(interp->sincLut);
+	free(interp->windowLut);
+}
+
+int16_t mInterpolatorSincInterpolate(const struct mInterpolator* interpolator, const struct mSampleBuffer* data, double time, double sampleStep) {
+	struct mInterpolatorSinc* interp = (struct mInterpolatorSinc*) interpolator;
+	ssize_t index = (ssize_t) time;
+	double subsample = time - floor(time);
+	unsigned step = sampleStep > 1 ? interp->resolution * sampleStep : interp->resolution;
+	unsigned yShift = subsample * step;
+	unsigned xShift = subsample * interp->resolution;
+	double sum = 0.0;
+	double kernelSum = 0.0;
+	double kernel;
+
+	ssize_t i;
+	for (i = 1 - (ssize_t) interp->width; i <= (ssize_t) interp->width; ++i) {
+		unsigned window = i * interp->resolution;
+		if (yShift > window) {
+			window = yShift - window;
+		} else {
+			window -= yShift;
+		}
+
+		unsigned sinc = i * step;
+		if (xShift > sinc) {
+			sinc = xShift - sinc;
+		} else {
+			sinc -= xShift;
+		}
+
+		kernel = interp->sincLut[sinc] * interp->windowLut[window];
+		kernelSum += kernel;
+		if (index + i >= 0 && index + i < (ssize_t) data->samples) {
+			sum += data->data[(index + i) * data->channels] * kernel;
+		}
+	}
+	return sum / kernelSum;
+}