From 2865c9ab2bf454041660d04508d82b1db436c26b Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Sat, 29 Sep 2018 23:11:16 -0700
Subject: [PATCH] Switch: Rumble support

---
 CHANGES                    |  1 +
 src/platform/switch/main.c | 58 ++++++++++++++++++++++++++++++++++++--
 2 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/CHANGES b/CHANGES
index 95361c9ef..577b69116 100644
--- a/CHANGES
+++ b/CHANGES
@@ -102,6 +102,7 @@ Changes from beta 1:
 Features:
  - Libretro: Add Game Boy cheat support
  - Qt: Separate fast forward volume control (fixes mgba.io/i/846, mgba.io/i/1143)
+ - Switch: Rumble support
 Bugfixes:
  - PSP2: Fix audio crackling after fast forward
  - PSP2: Fix audio crackling when buffer is full
diff --git a/src/platform/switch/main.c b/src/platform/switch/main.c
index 9fd7e3649..3888ff66c 100644
--- a/src/platform/switch/main.c
+++ b/src/platform/switch/main.c
@@ -75,6 +75,12 @@ static GLuint tex;
 
 static color_t* frameBuffer;
 static struct mAVStream stream;
+static struct mSwitchRumble {
+	struct mRumble d;
+	int up;
+	int down;
+	HidVibrationValue value;
+} rumble;
 static int audioBufferActive;
 static struct GBAStereoSample audioBuffer[N_BUFFERS][SAMPLES] __attribute__((__aligned__(0x1000)));
 static AudioOutBuffer audoutBuffer[N_BUFFERS];
@@ -82,6 +88,8 @@ static int enqueuedBuffers;
 static bool frameLimiter = true;
 static unsigned framecount = 0;
 static unsigned framecap = 10;
+static u32 vibrationDeviceHandles[4];
+static HidVibrationValue vibrationStop = { .freq_low = 160.f, .freq_high = 320.f };
 
 static bool initEgl() {
     s_display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
@@ -226,6 +234,7 @@ static void _setup(struct mGUIRunner* runner) {
 	_mapKey(&runner->core->inputMap, AUTO_INPUT, KEY_R, GBA_KEY_R);
 
 	runner->core->setVideoBuffer(runner->core, frameBuffer, 256);
+	runner->core->setPeripheral(runner->core, mPERIPH_RUMBLE, &rumble.d);
 	runner->core->setAVStream(runner->core, &stream);
 }
 
@@ -237,6 +246,18 @@ static void _gameLoaded(struct mGUIRunner* runner) {
 	blip_set_rates(runner->core->getAudioChannel(runner->core, 1), runner->core->frequency(runner->core), samplerate * ratio);
 
 	mCoreConfigGetUIntValue(&runner->config, "fastForwardCap", &framecap);
+
+	rumble.up = 0;
+	rumble.down = 0;
+}
+
+static void _gameUnloaded(struct mGUIRunner* runner) {
+	HidVibrationValue values[4];
+	memcpy(&values[0], &vibrationStop, sizeof(rumble.value));
+	memcpy(&values[1], &vibrationStop, sizeof(rumble.value));
+	memcpy(&values[2], &vibrationStop, sizeof(rumble.value));
+	memcpy(&values[3], &vibrationStop, sizeof(rumble.value));
+	hidSendVibrationValues(vibrationDeviceHandles, values, 4);
 }
 
 static void _drawTex(struct mGUIRunner* runner, unsigned width, unsigned height, bool faded) {
@@ -299,6 +320,24 @@ static void _drawFrame(struct mGUIRunner* runner, bool faded) {
 	glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
 
 	_drawTex(runner, width, height, faded);
+
+	HidVibrationValue values[4];
+	if (rumble.up) {
+		rumble.value.amp_low = rumble.up / (float) (rumble.up + rumble.down);
+		rumble.value.amp_high = rumble.up / (float) (rumble.up + rumble.down);
+		memcpy(&values[0], &rumble.value, sizeof(rumble.value));
+		memcpy(&values[1], &rumble.value, sizeof(rumble.value));
+		memcpy(&values[2], &rumble.value, sizeof(rumble.value));
+		memcpy(&values[3], &rumble.value, sizeof(rumble.value));
+	} else {
+		memcpy(&values[0], &vibrationStop, sizeof(rumble.value));
+		memcpy(&values[1], &vibrationStop, sizeof(rumble.value));
+		memcpy(&values[2], &vibrationStop, sizeof(rumble.value));
+		memcpy(&values[3], &vibrationStop, sizeof(rumble.value));
+	}
+	hidSendVibrationValues(vibrationDeviceHandles, values, 4);
+	rumble.up = 0;
+	rumble.down = 0;
 }
 
 static void _drawScreenshot(struct mGUIRunner* runner, const color_t* pixels, unsigned width, unsigned height, bool faded) {
@@ -355,6 +394,15 @@ static void _postAudioBuffer(struct mAVStream* stream, blip_t* left, blip_t* rig
 	++enqueuedBuffers;
 }
 
+void _setRumble(struct mRumble* rumble, int enable) {
+	struct mSwitchRumble* sr = (struct mSwitchRumble*) rumble;
+	if (enable) {
+		++sr->up;
+	} else {
+		++sr->down;
+	}
+}
+
 static int _batteryState(void) {
 	u32 charge;
 	int state = 0;
@@ -455,6 +503,12 @@ int main(int argc, char* argv[]) {
 	glEnableVertexAttribArray(offsetLocation);
 	glBindVertexArray(0);
 
+	rumble.d.setRumble = _setRumble;
+	rumble.value.freq_low = 120.0;
+	rumble.value.freq_high = 180.0;
+	hidInitializeVibrationDevices(&vibrationDeviceHandles[0], 2, CONTROLLER_HANDHELD, TYPE_HANDHELD | TYPE_JOYCON_PAIR);
+	hidInitializeVibrationDevices(&vibrationDeviceHandles[2], 2, CONTROLLER_PLAYER_1, TYPE_HANDHELD | TYPE_JOYCON_PAIR);
+
 	stream.videoDimensionsChanged = NULL;
 	stream.postVideoFrame = NULL;
 	stream.postAudioFrame = NULL;
@@ -552,11 +606,11 @@ int main(int argc, char* argv[]) {
 		.setup = _setup,
 		.teardown = NULL,
 		.gameLoaded = _gameLoaded,
-		.gameUnloaded = NULL,
+		.gameUnloaded = _gameUnloaded,
 		.prepareForFrame = _prepareForFrame,
 		.drawFrame = _drawFrame,
 		.drawScreenshot = _drawScreenshot,
-		.paused = NULL,
+		.paused = _gameUnloaded,
 		.unpaused = _gameLoaded,
 		.incrementScreenMode = NULL,
 		.setFrameLimiter = _setFrameLimiter,