From 9da95b509288e984eaee933a4b76ee15640626bb Mon Sep 17 00:00:00 2001
From: Paul Fd <paul@ferrand.cc>
Date: Mon, 10 Feb 2020 22:25:17 +0100
Subject: [PATCH] Add the SSE version (to test but it's not faster)

---
 benchmarks/BM_widthPos.cpp | 11 ++++++++++
 src/sfizz/SIMDHelpers.h    | 15 +++++++++++++
 src/sfizz/SIMDSSE.cpp      | 43 +++++++++++++++++++++++++++++++++++++-
 3 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/benchmarks/BM_widthPos.cpp b/benchmarks/BM_widthPos.cpp
index b55e5628..312a029a 100644
--- a/benchmarks/BM_widthPos.cpp
+++ b/benchmarks/BM_widthPos.cpp
@@ -64,6 +64,17 @@ BENCHMARK_DEFINE_F(WidthPosArray, Scalar)(benchmark::State& state) {
     }
 }
 
+BENCHMARK_DEFINE_F(WidthPosArray, SIMD)(benchmark::State& state) {
+    ScopedFTZ ftz;
+    const auto leftBuffer = absl::MakeSpan(left);
+    const auto rightBuffer = absl::MakeSpan(right);
+    for (auto _ : state)
+    {
+        sfz::width<float, true>(width, leftBuffer, rightBuffer);
+        sfz::pan<float, true>(position, leftBuffer, rightBuffer);
+    }
+}
 
 BENCHMARK_REGISTER_F(WidthPosArray, Scalar)->RangeMultiplier(4)->Range(1 << 2, 1 << 12);
+BENCHMARK_REGISTER_F(WidthPosArray, SIMD)->RangeMultiplier(4)->Range(1 << 2, 1 << 12);
 BENCHMARK_MAIN();
diff --git a/src/sfizz/SIMDHelpers.h b/src/sfizz/SIMDHelpers.h
index 355426d9..f4c5a2d7 100644
--- a/src/sfizz/SIMDHelpers.h
+++ b/src/sfizz/SIMDHelpers.h
@@ -802,6 +802,18 @@ void pan(absl::Span<const T> panEnvelope, absl::Span<T> leftBuffer, absl::Span<T
 template <>
 void pan<float, true>(absl::Span<const float> panEnvelope, absl::Span<float> leftBuffer, absl::Span<float> rightBuffer) noexcept;
 
+/**
+ * @brief Controls the width of a stereo signal, setting it to mono when width = 0 and inverting the channels
+ * when width = -1. Width = 1 has no effect.
+ *
+ * The output size will be the minimum of the width envelope span and left and right buffer span sizes.
+ *
+ * @tparam T the underlying type
+ * @tparam SIMD use the SIMD version or the scalar version
+ * @param panEnvelope
+ * @param leftBuffer
+ * @param rightBuffer
+ */
 template <class T, bool SIMD = SIMDConfig::pan>
 void width(absl::Span<const T> widthEnvelope, absl::Span<T> leftBuffer, absl::Span<T> rightBuffer) noexcept
 {
@@ -817,6 +829,9 @@ void width(absl::Span<const T> widthEnvelope, absl::Span<T> leftBuffer, absl::Sp
     }
 }
 
+template <>
+void width<float, true>(absl::Span<const float> widthEnvelope, absl::Span<float> leftBuffer, absl::Span<float> rightBuffer) noexcept;
+
 /**
  * @brief Computes the mean of a span
  *
diff --git a/src/sfizz/SIMDSSE.cpp b/src/sfizz/SIMDSSE.cpp
index 3093fcdf..ba98c12d 100644
--- a/src/sfizz/SIMDSSE.cpp
+++ b/src/sfizz/SIMDSSE.cpp
@@ -609,7 +609,7 @@ void sfz::pan<float, true>(absl::Span<const float> panEnvelope, absl::Span<float
         mmPan = _mm_mul_ps(mmPan, mmPiFour);
         sincos_ps(mmPan, &mmSin, &mmCos);
         auto mmLeft = _mm_mul_ps(mmCos, _mm_load_ps(left));
-        auto mmRight = _mm_mul_ps(mmCos, _mm_load_ps(left));
+        auto mmRight = _mm_mul_ps(mmSin, _mm_load_ps(right));
         _mm_store_ps(left, mmLeft);
         _mm_store_ps(right, mmRight);
         incrementAll<TypeAlignment>(pan, left, right);
@@ -621,6 +621,47 @@ void sfz::pan<float, true>(absl::Span<const float> panEnvelope, absl::Span<float
     }
 }
 
+template <>
+void sfz::width<float, true>(absl::Span<const float> widthEnvelope, absl::Span<float> leftBuffer, absl::Span<float> rightBuffer) noexcept
+{
+    ASSERT(leftBuffer.size() >= widthEnvelope.size());
+    ASSERT(rightBuffer.size() >= widthEnvelope.size());
+    auto* width = widthEnvelope.begin();
+    auto* left = leftBuffer.begin();
+    auto* right = rightBuffer.begin();
+    auto* sentinel = width + min(widthEnvelope.size(), leftBuffer.size(), rightBuffer.size());
+    const auto* lastAligned = prevAligned(sentinel);
+
+    while (unaligned(width, left, right) && width < lastAligned) {
+        _internals::snippetWidth(*width, *left, *right);
+        incrementAll(width, left, right);
+    }
+
+    const auto mmPiFour = _mm_set_ps1(piFour<float>);
+    __m128 mmCos;
+    __m128 mmSin;
+    while (width < lastAligned) {
+        auto mmWidth = _mm_load_ps(width);
+        mmWidth = _mm_mul_ps(mmWidth, mmPiFour);
+        sincos_ps(mmWidth, &mmSin, &mmCos);
+        auto mmCosPlusSine = _mm_add_ps(mmCos, mmSin);
+        auto mmCosMinusSine = _mm_sub_ps(mmCos, mmSin);
+        auto mmLeft = _mm_load_ps(left);
+        auto mmRight = _mm_load_ps(right);
+        auto mmTemp = _mm_mul_ps(mmCosMinusSine, mmRight);
+        mmRight = _mm_add_ps(_mm_mul_ps(mmCosMinusSine, mmLeft), _mm_mul_ps(mmCosPlusSine, mmRight));
+        mmLeft = _mm_add_ps(_mm_mul_ps(mmCosPlusSine, mmLeft), mmTemp);
+        _mm_store_ps(left, mmLeft);
+        _mm_store_ps(right, mmRight);
+        incrementAll<TypeAlignment>(width, left, right);
+    }
+
+    while (width < sentinel){
+        _internals::snippetWidth(*width, *left, *right);
+        incrementAll(width, left, right);
+    }
+}
+
 template <>
 float sfz::mean<float, true>(absl::Span<const float> vector) noexcept
 {