Fix some dither clipping issues

1 files changed, 11 insertions, 5 deletions

diff --git a/src/codecs/sample.cpp b/src/codecs/sample.cpp
index c5c96fb9..d4860b94 100644
--- a/src/codecs/sample.cpp
+++ b/src/codecs/sample.cpp
@@ -5,6 +5,7 @@
  */
 
 #include "sample.hpp"
+#include <stdint.h>
 
 #include <cstdint>
 
@@ -15,11 +16,16 @@ 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);
+auto shiftWithDither(int64_t src, uint_fast8_t bits) -> Sample {
+  // Generate `bits` random bits
+  uint64_t mask = 0xFFFFFFFF;
+  mask >>= 32 - bits;
+  int64_t noise = static_cast<int32_t>(komirand(&sSeed1, &sSeed2) & mask);
+  // Centre the noise around 0.
+  noise -= (mask >> 1);
+  // Apply to the sample, then clip and shift to 16 bit.
+  Sample clipped = Clip((src + noise) >> bits);
+  return clipped;
 }
 
 }  // namespace sample