From 83563168eaa84520409bd374b937aefd067f8a7d Mon Sep 17 00:00:00 2001
From: Iscle <albertiscle9@gmail.com>
Date: Sun, 18 Sep 2022 05:12:26 +0200
Subject: [PATCH] Implement HighShelf

---
 src/cpp/viper/utils/Biquad.h      | 12 ++++++---
 src/cpp/viper/utils/HighShelf.cpp | 42 ++++++++++++++++++++++---------
 src/cpp/viper/utils/HighShelf.h   | 31 ++++++++++++-----------
 3 files changed, 56 insertions(+), 29 deletions(-)

diff --git a/src/cpp/viper/utils/Biquad.h b/src/cpp/viper/utils/Biquad.h
index 6b6b586..f2e7ee3 100644
--- a/src/cpp/viper/utils/Biquad.h
+++ b/src/cpp/viper/utils/Biquad.h
@@ -1,6 +1,5 @@
 #pragma once
 
-
 #include <cstdint>
 
 class Biquad {
@@ -15,6 +14,13 @@ public:
     void SetLowPassParameter(double frequency, double samplingRate, double qFactor);
 
 private:
-    double y_2, y_1, x_2, x_1;
-    double b0, b1, b2, a1, a2;
+    double x_1;
+    double x_2;
+    double y_1;
+    double y_2;
+    double a1;
+    double a2;
+    double b0;
+    double b1;
+    double b2;
 };
diff --git a/src/cpp/viper/utils/HighShelf.cpp b/src/cpp/viper/utils/HighShelf.cpp
index d5ea231..227267e 100644
--- a/src/cpp/viper/utils/HighShelf.cpp
+++ b/src/cpp/viper/utils/HighShelf.cpp
@@ -1,9 +1,8 @@
-#include <cmath>
 #include "HighShelf.h"
+#include <cmath>
 
-float HighShelf::Process(float sample) {
-    float out = sample * this->b0 + this->x_1 * this->b1 + this->x_2 * this->b2 + this->y_1 * this->a1 +
-                this->y_2 * this->a2;
+double HighShelf::Process(double sample) {
+    double out = (((this->x_1 * this->b1 + sample * this->b0 + this->b2 * this->x_2) - this->y_1 * this->a1) - this->a2 * this->y_2) * this->a0;
     this->y_2 = this->y_1;
     this->y_1 = out;
     this->x_2 = this->x_1;
@@ -11,18 +10,37 @@ float HighShelf::Process(float sample) {
     return out;
 }
 
-void HighShelf::SetFrequency(uint32_t freq) {
+void HighShelf::SetFrequency(double freq) {
     this->frequency = freq;
 }
 
-void HighShelf::SetGain(float gain) {
-    this->gain = 20.f * log10f(gain);
+void HighShelf::SetGain(double gain) {
+    this->gain = log10(gain) * 20.0;
 }
 
-void HighShelf::SetQuality(float q) {
-    this->quality = q;
-}
+void HighShelf::SetSamplingRate(double samplingRate) {
+    double x = (2 * M_PI * this->frequency) / samplingRate;
+    double sinX = sin(x);
+    double cosX = cos(x);
+    double y = exp((this->gain * log(10.0)) / 40.0);
 
-void HighShelf::SetSamplingRate(uint32_t samplerate) {
-    this->samplerate = samplerate;
+    this->x_1 = 0.0;
+    this->x_2 = 0.0;
+    this->y_1 = 0.0;
+    this->y_2 = 0.0;
+
+    double z = sqrt(y * 2.0) * sinX;
+    double a = (y - 1.0) * cosX;
+    double b = (y + 1.0) - a;
+    double c = z + b;
+    double d = (y + 1.0) * cosX;
+    double e = (y + 1.0) + a;
+    double f = (y - 1.0) - d;
+
+    this->a0 = 1.0 / c;
+    this->a1 = a * 2.0;
+    this->a2 = b - z;
+    this->b0 = (e + z) * y;
+    this->b1 = -y * 2.0 * ((y - 1.0) + d);
+    this->b2 = (e - z) * y;
 }
diff --git a/src/cpp/viper/utils/HighShelf.h b/src/cpp/viper/utils/HighShelf.h
index 2bd1258..61df8b0 100644
--- a/src/cpp/viper/utils/HighShelf.h
+++ b/src/cpp/viper/utils/HighShelf.h
@@ -4,21 +4,24 @@
 
 class HighShelf {
 public:
-    float Process(float sample);
+    double Process(double sample);
+    void SetFrequency(double freq);
+    void SetGain(double gain);
+    void SetSamplingRate(double samplingRate);
 
-    void SetFrequency(uint32_t freq);
-
-    void SetGain(float gain);
-
-    void SetQuality(float q);
-
-    void SetSamplingRate(uint32_t samplerate);
-
-    uint32_t frequency, samplerate;
-    float quality, gain;
-
-    float y_2, y_1, x_2, x_1;
-    float b0, b1, b2, a1, a2;
+private:
+    double frequency;
+    double gain;
+    double x_1;
+    double x_2;
+    double y_1;
+    double y_2;
+    double b0;
+    double b1;
+    double b2;
+    double a0;
+    double a1;
+    double a2;
 };