From 2e00ffbebaf43b4c428c2b81f93accb621f84fbc Mon Sep 17 00:00:00 2001
From: heck <heck@heck.live>
Date: Thu, 26 Sep 2024 22:53:48 +0200
Subject: [PATCH] libDizzy: Add module 'density'

---
 src/density.cc | 110 +++++++++++++++++++++++++++++++++++++++++++++++++
 src/density.hh |  33 +++++++++++++++
 2 files changed, 143 insertions(+)
 create mode 100644 src/density.cc
 create mode 100644 src/density.hh

diff --git a/src/density.cc b/src/density.cc
new file mode 100644
index 0000000..8892915
--- /dev/null
+++ b/src/density.cc
@@ -0,0 +1,110 @@
+#include "density.hh"
+namespace Heck {
+
+    void Density::process(const f32 *inSourceP, f32 *inDestP, u32 size)
+    {
+        UInt32 nSampleFrames = size;
+        const Float32 *sourceP = inSourceP;
+        Float32 *destP = inDestP;
+        Float64 inputSample;
+        Float64 drySample;
+        Float64 overallscale = 1.0;
+        overallscale /= 44100.0;
+        overallscale *= Constants::AUDIO_SAMPLERATE;
+        Float64 density = params[0];
+        Float64 iirAmount = pow(params[1], 3) / overallscale;
+        Float64 output = params[2];
+        Float64 wet = params[3];
+        Float64 dry = 1.0 - wet;
+        Float64 bridgerectifier;
+        Float64 out = fabs(density);
+        density = density * fabs(density);
+        Float64 count;
+
+        while (nSampleFrames-- > 0) {
+            inputSample = *sourceP;
+
+            if (fabs(inputSample) < 1.18e-23) {
+                inputSample = fpd * 1.18e-17;
+            }
+
+            drySample = inputSample;
+
+            if (fpFlip) {
+                iirSampleA = (iirSampleA * (1 - iirAmount)) + (inputSample * iirAmount);
+                inputSample -= iirSampleA;
+            } else {
+                iirSampleB = (iirSampleB * (1 - iirAmount)) + (inputSample * iirAmount);
+                inputSample -= iirSampleB;
+            }
+
+            //highpass section
+            count = density;
+            while (count > 1.0) {
+                bridgerectifier = fabs(inputSample) * 1.57079633;
+                if (bridgerectifier > 1.57079633) {
+                    bridgerectifier = 1.57079633;
+                }
+                //max value for sine function
+                bridgerectifier = sin(bridgerectifier);
+                if (inputSample > 0.0) {
+                    inputSample = bridgerectifier;
+                } else {
+                    inputSample = -bridgerectifier;
+                }
+                count = count - 1.0;
+            }
+
+            //we have now accounted for any really high density settings.
+            while (out > 1.0) {
+                out = out - 1.0;
+            }
+            bridgerectifier = fabs(inputSample) * 1.57079633;
+            if (bridgerectifier > 1.57079633) {
+                bridgerectifier = 1.57079633;
+            }
+            //max value for sine function
+            if (density > 0) {
+                bridgerectifier = sin(bridgerectifier);
+            } else {
+                bridgerectifier = 1 - cos(bridgerectifier);
+            }
+            //produce either boosted or starved version
+            if (inputSample > 0) {
+                inputSample = (inputSample * (1 - out)) + (bridgerectifier * out);
+            } else {
+                inputSample = (inputSample * (1 - out)) - (bridgerectifier * out);
+            }
+            //blend according to density control
+
+            if (output < 1.0) {
+                inputSample *= output;
+            }
+
+            if (wet < 1.0) {
+                inputSample = (drySample * dry) + (inputSample * wet);
+            }
+
+            //nice little output stage template: if we have another scale of floating point
+            //number, we really don't want to meaninglessly multiply that by 1.0.
+            fpFlip = !fpFlip;
+
+            //begin 32 bit floating point dither
+
+            if constexpr (DO_DITHER) {
+                int expon;
+                frexpf((float)inputSample, &expon);
+                fpd ^= fpd << 13;
+                fpd ^= fpd >> 17;
+                fpd ^= fpd << 5;
+                inputSample += ((double(fpd) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
+                //end 32 bit floating point dither
+            }
+            *destP = inputSample;
+
+            //that simple.
+            sourceP++;
+            destP++;
+        }
+    }
+} // namespace Heck
\ No newline at end of file
diff --git a/src/density.hh b/src/density.hh
new file mode 100644
index 0000000..7de2b26
--- /dev/null
+++ b/src/density.hh
@@ -0,0 +1,33 @@
+#ifndef HECK_OSP_DENSITY
+#define HECK_OSP_DENSITY
+
+#include "types.hh"
+#include <array>
+
+namespace Heck::Constants {
+    constexpr Samplerate AUDIO_SAMPLERATE = Samplerate::SAI_48KHZ;
+}
+
+namespace Heck {
+
+    struct Density {
+        using Float32 = f32;
+        using Float64 = f64;
+        using UInt32 = u32;
+
+        static constexpr bool DO_DITHER{ false };
+
+        std::array<f64, 4> params{ 0., 0., 1., 1. };
+
+        void process(const f32 * const inSourceP, f32 *inDestP, u32 size);
+
+    private:
+        Float64 iirSampleA;
+        Float64 iirSampleB;
+        uint32_t fpd;
+        bool fpFlip;
+    };
+
+
+} // namespace Heck
+#endif
\ No newline at end of file