Add dither when requantising >16 bit samples

4 files changed, 41 insertions, 13 deletions

diff --git a/src/codecs/CMakeLists.txt b/src/codecs/CMakeLists.txt
index d42c7426..bd4ec046 100644
--- a/src/codecs/CMakeLists.txt
+++ b/src/codecs/CMakeLists.txt
@@ -4,8 +4,9 @@
 
 idf_component_register(
   SRCS "codec.cpp" "mad.cpp" "miniflac.cpp" "opus.cpp" "vorbis.cpp"
-       "source_buffer.cpp"
+       "source_buffer.cpp" "sample.cpp"
   INCLUDE_DIRS "include"
-  REQUIRES "result" "span" "libmad" "miniflac" "tremor" "opusfile" "memory")
+  REQUIRES "result" "span" "libmad" "miniflac" "tremor" "opusfile" "memory"
+       "komihash")
 
 target_compile_options("${COMPONENT_LIB}" PRIVATE ${EXTRA_WARNINGS})
diff --git a/src/codecs/include/sample.hpp b/src/codecs/include/sample.hpp
index 937b6b79..8d79a228 100644
--- a/src/codecs/include/sample.hpp
+++ b/src/codecs/include/sample.hpp
@@ -25,15 +25,14 @@ namespace sample {
 typedef int16_t Sample;
 
 constexpr auto Clip(int64_t v) -> Sample {
-  if (v > INT16_MAX)
-    return INT16_MAX;
-  if (v < INT16_MIN)
-    return INT16_MIN;
-  return v;
+  return std::clamp<int64_t>(v, INT16_MIN, INT16_MAX);
 }
 
+auto applyDither(int64_t src, uint_fast8_t bits) -> int32_t;
+
 constexpr auto FromSigned(int32_t src, uint_fast8_t bits) -> Sample {
   if (bits > 16) {
+    src = applyDither(src, bits - 16);
     return src >> (bits - sizeof(Sample) * 8);
   } else if (bits < 16) {
     return src << (sizeof(Sample) * 8 - bits);
@@ -48,16 +47,20 @@ constexpr auto FromUnsigned(uint32_t src, uint_fast8_t bits) -> Sample {
 }
 
 constexpr auto FromFloat(float src) -> Sample {
-  return std::clamp<float>(src, -1.0f, 1.0f) * static_cast<float>(INT16_MAX);
+  int32_t quantised =
+      std::clamp<float>(src, -1.0f, 1.0f) * static_cast<float>(INT32_MAX);
+  return FromSigned(quantised, 32);
 }
 
 constexpr auto FromDouble(double src) -> Sample {
-  return std::clamp<double>(src, -1.0, 1.0) * static_cast<double>(INT16_MAX);
+  int32_t quantised =
+      std::clamp<double>(src, -1.0, 1.0) * static_cast<double>(INT32_MAX);
+  return FromSigned(quantised, 32);
 }
 
 constexpr auto FromMad(mad_fixed_t src) -> Sample {
   // Round the bottom bits.
-  src += (1L << (MAD_F_FRACBITS - 16));
+  src += (1L << (MAD_F_FRACBITS - 24));
 
   // Clip the leftover bits to within range.
   if (src >= MAD_F_ONE)
@@ -66,7 +69,7 @@ constexpr auto FromMad(mad_fixed_t src) -> Sample {
     src = -MAD_F_ONE;
 
   // Quantize.
-  return FromSigned(src >> (MAD_F_FRACBITS + 1 - 16), 16);
+  return FromSigned(src >> (MAD_F_FRACBITS + 1 - 24), 24);
 }
 
 static constexpr float kFactor = 1.0f / static_cast<float>(INT16_MAX);
diff --git a/src/codecs/miniflac.cpp b/src/codecs/miniflac.cpp
index e4eadeca..ace73466 100644
--- a/src/codecs/miniflac.cpp
+++ b/src/codecs/miniflac.cpp
@@ -5,9 +5,8 @@
  */
 
 #include "miniflac.hpp"
-#include <stdint.h>
-#include <sys/_stdint.h>
 
+#include <cstdint>
 #include <cstdlib>
 
 #include "esp_heap_caps.h"
diff --git a/src/codecs/sample.cpp b/src/codecs/sample.cpp
new file mode 100644
index 00000000..c5c96fb9
--- /dev/null
+++ b/src/codecs/sample.cpp
@@ -0,0 +1,25 @@
+/*
+ * Copyright 2023 jacqueline <me@jacqueline.id.au>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "sample.hpp"
+
+#include <cstdint>
+
+#include "komihash.h"
+
+namespace sample {
+
+static uint64_t sSeed1{0};
+static uint64_t sSeed2{0};
+
+auto applyDither(int64_t src, uint_fast8_t bits) -> int32_t {
+  uint64_t mask = 0xFFFFFFFF;                          // 32 ones
+  mask >>= 32 - bits;                                  // `bits` ones
+  uint64_t noise = komirand(&sSeed1, &sSeed2) & mask;  // `bits` random noise
+  return std::clamp<int64_t>(src + noise, INT32_MIN, INT32_MAX);
+}
+
+}  // namespace sample