Fix haptics scaling to match VibrationEffect function

Fix the scale function in ExternalVibrationUtils to match the one used
by VibrationEffect. Add flagged fix with new tests for the module
libvibrator.

Bug: 356144312
Flag: android.os.vibrator.fix_audio_coupled_haptics_scaling
Test: libvibrator_test
Change-Id: I56116536d7cddab29448f0e436ee752c7181eb45
Merged-In: I56116536d7cddab29448f0e436ee752c7181eb45
diff --git a/libs/vibrator/ExternalVibrationUtils.cpp b/libs/vibrator/ExternalVibrationUtils.cpp
index 761ac1b..706f3d7 100644
--- a/libs/vibrator/ExternalVibrationUtils.cpp
+++ b/libs/vibrator/ExternalVibrationUtils.cpp
@@ -15,6 +15,9 @@
  */
 #include <cstring>
 
+#include <android_os_vibrator.h>
+
+#include <algorithm>
 #include <math.h>
 
 #include <vibrator/ExternalVibrationUtils.h>
@@ -25,8 +28,9 @@
 static constexpr float HAPTIC_SCALE_VERY_LOW_RATIO = 2.0f / 3.0f;
 static constexpr float HAPTIC_SCALE_LOW_RATIO = 3.0f / 4.0f;
 static constexpr float HAPTIC_MAX_AMPLITUDE_FLOAT = 1.0f;
+static constexpr float SCALE_GAMMA = 0.65f; // Same as VibrationEffect.SCALE_GAMMA
 
-float getHapticScaleGamma(HapticLevel level) {
+float getOldHapticScaleGamma(HapticLevel level) {
     switch (level) {
     case HapticLevel::VERY_LOW:
         return 2.0f;
@@ -41,7 +45,7 @@
     }
 }
 
-float getHapticMaxAmplitudeRatio(HapticLevel level) {
+float getOldHapticMaxAmplitudeRatio(HapticLevel level) {
     switch (level) {
     case HapticLevel::VERY_LOW:
         return HAPTIC_SCALE_VERY_LOW_RATIO;
@@ -56,6 +60,52 @@
     }
 }
 
+/* Same as VibrationScaler.SCALE_LEVEL_* */
+float getHapticScaleFactor(HapticLevel level) {
+    switch (level) {
+        case HapticLevel::VERY_LOW:
+            return 0.6f;
+        case HapticLevel::LOW:
+            return 0.8f;
+        case HapticLevel::HIGH:
+            return 1.2f;
+        case HapticLevel::VERY_HIGH:
+            return 1.4f;
+        default:
+            return 1.0f;
+    }
+}
+
+float applyOldHapticScale(float value, float gamma, float maxAmplitudeRatio) {
+    float sign = value >= 0 ? 1.0 : -1.0;
+    return powf(fabsf(value / HAPTIC_MAX_AMPLITUDE_FLOAT), gamma)
+                * maxAmplitudeRatio * HAPTIC_MAX_AMPLITUDE_FLOAT * sign;
+}
+
+float applyNewHapticScale(float value, float scaleFactor) {
+    float scale = powf(scaleFactor, 1.0f / SCALE_GAMMA);
+    if (scaleFactor <= 1) {
+        // Scale down is simply a gamma corrected application of scaleFactor to the intensity.
+        // Scale up requires a different curve to ensure the intensity will not become > 1.
+        return value * scale;
+    }
+
+    float sign = value >= 0 ? 1.0f : -1.0f;
+    float extraScale = powf(scaleFactor, 4.0f - scaleFactor);
+    float x = fabsf(value) * scale * extraScale;
+    float maxX = scale * extraScale; // scaled x for intensity == 1
+
+    float expX = expf(x);
+    float expMaxX = expf(maxX);
+
+    // Using f = tanh as the scale up function so the max value will converge.
+    // a = 1/f(maxX), used to scale f so that a*f(maxX) = 1 (the value will converge to 1).
+    float a = (expMaxX + 1.0f) / (expMaxX - 1.0f);
+    float fx = (expX - 1.0f) / (expX + 1.0f);
+
+    return sign * std::clamp(a * fx, 0.0f, 1.0f);
+}
+
 void applyHapticScale(float* buffer, size_t length, HapticScale scale) {
     if (scale.isScaleMute()) {
         memset(buffer, 0, length * sizeof(float));
@@ -65,15 +115,18 @@
         return;
     }
     HapticLevel hapticLevel = scale.getLevel();
+    float scaleFactor = getHapticScaleFactor(hapticLevel);
     float adaptiveScaleFactor = scale.getAdaptiveScaleFactor();
-    float gamma = getHapticScaleGamma(hapticLevel);
-    float maxAmplitudeRatio = getHapticMaxAmplitudeRatio(hapticLevel);
+    float oldGamma = getOldHapticScaleGamma(hapticLevel);
+    float oldMaxAmplitudeRatio = getOldHapticMaxAmplitudeRatio(hapticLevel);
 
     for (size_t i = 0; i < length; i++) {
         if (hapticLevel != HapticLevel::NONE) {
-            float sign = buffer[i] >= 0 ? 1.0 : -1.0;
-            buffer[i] = powf(fabsf(buffer[i] / HAPTIC_MAX_AMPLITUDE_FLOAT), gamma)
-                        * maxAmplitudeRatio * HAPTIC_MAX_AMPLITUDE_FLOAT * sign;
+            if (android_os_vibrator_fix_audio_coupled_haptics_scaling()) {
+                buffer[i] = applyNewHapticScale(buffer[i], scaleFactor);
+            } else {
+                buffer[i] = applyOldHapticScale(buffer[i], oldGamma, oldMaxAmplitudeRatio);
+            }
         }
 
         if (adaptiveScaleFactor != 1.0f) {