From 6448a692e297e71fc86b2f07fd142c0515fb6406 Mon Sep 17 00:00:00 2001
From: Lior Halphon <LIJI32@gmail.com>
Date: Wed, 29 Apr 2020 16:06:11 +0300
Subject: [PATCH] Add smart rumble to games without a rumblepak

---
 Core/apu.c     |  1 -
 Core/display.c |  8 +-------
 Core/gb.h      |  1 +
 Core/rumble.c  | 42 ++++++++++++++++++++++++++++++++++++++++++
 Core/rumble.h  |  8 ++++++++
 5 files changed, 52 insertions(+), 8 deletions(-)
 create mode 100644 Core/rumble.c
 create mode 100644 Core/rumble.h

diff --git a/Core/apu.c b/Core/apu.c
index e63d89f5..afb970c8 100644
--- a/Core/apu.c
+++ b/Core/apu.c
@@ -493,7 +493,6 @@ void GB_apu_run(GB_gameboy_t *gb)
 
                 /* Step LFSR */
                 unsigned high_bit_mask = gb->apu.noise_channel.narrow ? 0x4040 : 0x4000;
-                /* Todo: is this formula is different on a GBA? */
                 bool new_high_bit = (gb->apu.noise_channel.lfsr ^ (gb->apu.noise_channel.lfsr >> 1) ^ 1) & 1;
                 gb->apu.noise_channel.lfsr >>= 1;
 
diff --git a/Core/display.c b/Core/display.c
index b3522f11..be2e1088 100644
--- a/Core/display.c
+++ b/Core/display.c
@@ -200,14 +200,8 @@ static void display_vblank(GB_gameboy_t *gb)
             }
         }
     }
+    GB_handle_rumble(gb);
 
-    if (gb->rumble_callback) {
-        if (gb->rumble_on_cycles + gb->rumble_off_cycles) {
-            gb->rumble_callback(gb, gb->rumble_on_cycles / (double)(gb->rumble_on_cycles + gb->rumble_off_cycles));
-            gb->rumble_on_cycles = gb->rumble_off_cycles = 0;
-        }
-    }
-    
     if (gb->vblank_callback) {
         gb->vblank_callback(gb);
     }
diff --git a/Core/gb.h b/Core/gb.h
index d63d400d..7967b6f5 100644
--- a/Core/gb.h
+++ b/Core/gb.h
@@ -22,6 +22,7 @@
 #include "symbol_hash.h"
 #include "sgb.h"
 #include "cheats.h"
+#include "rumble.h"
 
 #define GB_STRUCT_VERSION 13
 
diff --git a/Core/rumble.c b/Core/rumble.c
new file mode 100644
index 00000000..5ac3d0d5
--- /dev/null
+++ b/Core/rumble.c
@@ -0,0 +1,42 @@
+#include "rumble.h"
+#include "gb.h"
+
+void GB_handle_rumble(GB_gameboy_t *gb)
+{
+    if (gb->rumble_callback) {
+        if (gb->cartridge_type->has_rumble) {
+            if (gb->rumble_on_cycles + gb->rumble_off_cycles) {
+                gb->rumble_callback(gb, gb->rumble_on_cycles / (double)(gb->rumble_on_cycles + gb->rumble_off_cycles));
+                gb->rumble_on_cycles = gb->rumble_off_cycles = 0;
+            }
+        }
+        else {
+            unsigned volume = (gb->io_registers[GB_IO_NR50] & 7) + 1 + ((gb->io_registers[GB_IO_NR50] >> 4) & 7) + 1;
+            unsigned ch4_volume = volume * (!!(gb->io_registers[GB_IO_NR51] & 8) + !!(gb->io_registers[GB_IO_NR51] & 0x80));
+            unsigned ch1_volume = volume * (!!(gb->io_registers[GB_IO_NR51] & 1) + !!(gb->io_registers[GB_IO_NR51] & 0x10));
+            
+            double ch4_rumble = (MIN(gb->apu.noise_channel.sample_length * (gb->apu.noise_channel.narrow? 8 : 1) , 4096) * ((signed) gb->apu.noise_channel.current_volume * gb->apu.noise_channel.current_volume * ch4_volume / 32.0 - 50) - 2048) / 2048.0;
+            
+            ch4_rumble = MIN(ch4_rumble, 1.0);
+            ch4_rumble = MAX(ch4_rumble, 0.0);
+            
+            double ch1_rumble = 0;
+            if (gb->apu.sweep_enabled && ((gb->io_registers[GB_IO_NR10] >> 4) & 7)) {
+                double sweep_speed = (gb->io_registers[GB_IO_NR10] & 7) / (double)((gb->io_registers[GB_IO_NR10] >> 4) & 7);
+                ch1_rumble = gb->apu.square_channels[GB_SQUARE_1].current_volume * ch1_volume / 32.0 * sweep_speed / 8.0 - 0.5;
+                ch1_rumble = MIN(ch1_rumble, 1.0);
+                ch1_rumble = MAX(ch1_rumble, 0.0);
+            }
+            
+            if (!gb->apu.is_active[GB_NOISE]) {
+                ch4_rumble = 0;
+            }
+            
+            if (!gb->apu.is_active[GB_SQUARE_1]) {
+                ch1_rumble = 0;
+            }
+            
+            gb->rumble_callback(gb, MIN(MAX(ch1_rumble / 2 + ch4_rumble, 0.0), 1.0));
+        }
+    }
+}
diff --git a/Core/rumble.h b/Core/rumble.h
new file mode 100644
index 00000000..a378f2d8
--- /dev/null
+++ b/Core/rumble.h
@@ -0,0 +1,8 @@
+#ifndef rumble_h
+#define rumble_h
+
+#include "gb_struct_def.h"
+
+void GB_handle_rumble(GB_gameboy_t *gb);
+
+#endif /* rumble_h */