From db6b1a9301567b60d0a73cf5af31c650d98d5c76 Mon Sep 17 00:00:00 2001
From: Shaun Inman <shaun@panic.com>
Date: Wed, 15 Feb 2023 08:50:00 -0500
Subject: [PATCH] moved rumble to thread + prevent rapid vacillation

---
 src/common/api.c   | 57 +++++++++++++++++++++++++++++++++++++++++++---
 src/common/api.h   |  9 +++++++-
 src/minarch/main.c | 12 +++++++---
 3 files changed, 71 insertions(+), 7 deletions(-)

diff --git a/src/common/api.c b/src/common/api.c
index b24e4c7..a288ae9 100644
--- a/src/common/api.c
+++ b/src/common/api.c
@@ -6,6 +6,7 @@
 
 #include <fcntl.h>
 #include <unistd.h>
+#include <pthread.h>
 
 #include <SDL/SDL.h>
 #include <SDL/SDL_image.h>
@@ -1005,6 +1006,58 @@ int PAD_justRepeated(int btn)	{ return pad.just_repeated & btn; }
 
 ///////////////////////////////
 
+
+
+static struct VIB_Context {
+	pthread_t pt;
+	int queued_strength;
+	int strength;
+} vib;
+static void* VIB_thread(void *arg) {
+	char buffer[4];
+#define DEFER_FRAMES 3
+	static int defer = 0;
+	while(1) {
+		SDL_Delay(17);
+		if (vib.queued_strength!=vib.strength) {
+			if (defer<DEFER_FRAMES && vib.queued_strength==0) { // minimize vacillation between 0 and some number (which this motor doesn't like)
+				defer += 1;
+				continue;
+			}
+			vib.strength = vib.queued_strength;
+			defer = 0;
+
+			int val = MAX(0, MIN((100 * (vib.strength+1))>>16, 100));
+			sprintf(buffer, "%i", val);
+
+			int fd = open("/sys/class/power_supply/battery/moto", O_WRONLY);
+			if (fd>0) {
+				write(fd, buffer, strlen(buffer));
+				close(fd);
+			}
+		}
+	}
+	return 0;
+}
+void VIB_init(void) {
+	vib.queued_strength = vib.strength = 0;
+	pthread_create(&vib.pt, NULL, &VIB_thread, NULL);
+}
+void VIB_quit(void) {
+	VIB_setStrength(0);
+	pthread_cancel(vib.pt);
+	pthread_join(vib.pt, NULL);
+}
+void VIB_setStrength(int strength) {
+	if (vib.queued_strength==strength) return;
+	vib.queued_strength = strength;
+}
+int VIB_getStrength(void) {
+	return vib.strength;
+}
+
+///////////////////////////////
+
 // TODO: separate settings/battery and power management?
 void POW_update(int* _dirty, int* _show_setting, POW_callback_t before_sleep, POW_callback_t after_sleep) {
 	int dirty = _dirty ? *_dirty : 0;
@@ -1174,6 +1227,7 @@ int POW_isCharging(void) {
 	return getInt("/sys/class/power_supply/battery/charger_online");
 }
 int POW_getBattery(void) { // 5-100 in 25% fragments
+	// TODO: move to infrequently called thread
 	int i = getInt("/sys/class/power_supply/battery/voltage_now") / 10000; // 310-410
 	i -= 310; 	// ~0-100
 	
@@ -1185,6 +1239,3 @@ int POW_getBattery(void) { // 5-100 in 25% fragments
 	if (i>10)  return 20;
 	else      return  10;
 }
-void POW_setRumble(int strength) {
-	putInt("/sys/class/power_supply/battery/moto", strength);
-}
\ No newline at end of file
diff --git a/src/common/api.h b/src/common/api.h
index abc1a70..d872d67 100644
--- a/src/common/api.h
+++ b/src/common/api.h
@@ -198,6 +198,14 @@ int PAD_isPressed(int btn);
 int PAD_justReleased(int btn);
 int PAD_justRepeated(int btn);
 
+
+///////////////////////////////
+
+void VIB_init(void);
+void VIB_quit(void);
+void VIB_setStrength(int strength);
+ int VIB_getStrength(void);
+	
 ///////////////////////////////
 
 // TODO: rename PLAT_*?
@@ -212,7 +220,6 @@ void POW_enableAutosleep(void);
 int POW_preventAutosleep(void);
 int POW_isCharging(void);
 int POW_getBattery(void);
-void POW_setRumble(int strength); // 0-100
 
 #define CPU_SPEED_MENU			 504000 // 240000 was having latency issues
 #define CPU_SPEED_POWERSAVE 	1104000
diff --git a/src/minarch/main.c b/src/minarch/main.c
index bae8554..443e3bb 100644
--- a/src/minarch/main.c
+++ b/src/minarch/main.c
@@ -197,7 +197,7 @@ static void Game_open(char* path) {
 }
 static void Game_close(void) {
 	if (game.data) free(game.data);
-	POW_setRumble(0); // just in case
+	VIB_setStrength(0); // just in case
 }
 
 static struct retro_disk_control_ext_callback disk_control_ext;
@@ -1222,7 +1222,7 @@ void Input_init(const struct retro_input_descriptor *vars) {
 
 static bool set_rumble_state(unsigned port, enum retro_rumble_effect effect, uint16_t strength) {
 	// TODO: handle other args? not sure I can
-	POW_setRumble(strength);
+	VIB_setStrength(strength);
 }
 static bool environment_callback(unsigned cmd, void *data) { // copied from picoarch initially
 	// printf("environment_callback: %i\n", cmd); fflush(stdout);
@@ -2557,7 +2557,7 @@ void Core_load(void) {
 	if (a<=0) a = (double)av_info.geometry.base_width / av_info.geometry.base_height;
 	core.aspect_ratio = a;
 	
-	LOG_info("aspect_ratio: %f\n", a);
+	LOG_info("aspect_ratio: %f fps: %f\n", a, core.fps);
 }
 void Core_reset(void) {
 	core.reset();
@@ -3434,6 +3434,9 @@ static void Menu_loop(void) {
 	POW_setCPUSpeed(CPU_SPEED_MENU); // set Hz directly
 	GFX_setVsync(VSYNC_STRICT);
 	
+	int rumble_strength = VIB_getStrength();
+	VIB_setStrength(0);
+	
 	fast_forward = 0;
 	POW_enableAutosleep();
 	PAD_reset();
@@ -3791,6 +3794,7 @@ static void Menu_loop(void) {
 
 		GFX_setVsync(prevent_tearing); // restore vsync value
 		setOverclock(overclock); // restore overclock value
+		if (rumble_strength) VIB_setStrength(rumble_strength);
 	}
 		
 	SDL_FreeSurface(backing);
@@ -3881,6 +3885,7 @@ int main(int argc , char* argv[]) {
 	getEmuName(rom_path, tag_name);
 	
 	screen = GFX_init(MODE_MENU);
+	VIB_init();
 	
 	MSG_init();
 	InitSettings();
@@ -3931,6 +3936,7 @@ int main(int argc , char* argv[]) {
 	SDL_FreeSurface(screen);
 	MSG_quit();
 	QuitSettings();
+	VIB_quit();
 	GFX_quit();
 	
 	return EXIT_SUCCESS;