From cccf9d8f3a6e52f8d2875eb77280c9c3d7f19147 Mon Sep 17 00:00:00 2001
From: DevMiner <devminer@devminer.xyz>
Date: Fri, 8 Mar 2024 13:49:06 +0100
Subject: [PATCH] impl(network): toggling mags

---
 src/network/connection.cpp | 51 ++++++++++++++++++++++++++++++++++++++
 src/network/connection.h   |  3 +++
 src/network/packets.h      |  1 +
 3 files changed, 55 insertions(+)

diff --git a/src/network/connection.cpp b/src/network/connection.cpp
index 1f60ad876..109c93d8a 100644
--- a/src/network/connection.cpp
+++ b/src/network/connection.cpp
@@ -299,6 +299,15 @@ void Connection::sendSensorInfo(Sensor* sensor) {
 	MUST(sendByte((uint8_t)sensor->getSensorState()));
 	MUST(sendByte(sensor->getSensorType()));
 
+	// 0b00000000
+	//         ^^
+	//         |`- supportsMagToggle
+	//         `-- hasMagEnabled
+	const bool supportsMagToggle = sensor->supportsTogglingMagnetometer();
+	const bool hasMagEnabled = sensor->hasMagnetometerEnabled();
+	const uint8_t magInfo = (supportsMagToggle << 1) | hasMagEnabled;
+	MUST(sendByte(magInfo));
+
 	MUST(endPacket());
 }
 
@@ -409,6 +418,19 @@ void Connection::sendTrackerDiscovery() {
 	MUST(endPacket());
 }
 
+void Connection::sendToggleMagnetometerResult(uint8_t sensorId, bool result) {
+	MUST(m_Connected);
+
+	MUST(beginPacket());
+
+	MUST(sendPacketType(PACKET_TOGGLE_MAGNETOMETER));
+	MUST(sendPacketNumber());
+	MUST(sendByte(sensorId));
+	MUST(sendByte(result));
+
+	MUST(endPacket());
+}
+
 #if ENABLE_INSPECTION
 void Connection::sendInspectionRawIMUData(
 	uint8_t sensorId,
@@ -709,6 +731,35 @@ void Connection::update() {
 				#endif
 			}
 
+			break;
+
+		case PACKET_TOGGLE_MAGNETOMETER:
+			constexpr size_t PACKET_TOGGLE_MAGNETOMETER_SIZE = 4 + 8 + 1 + 1;
+
+			// Packet type (4) + Packet number (8) + sensor ID (1) + enabled (1)
+			if (len < PACKET_TOGGLE_MAGNETOMETER_SIZE) {
+				m_Logger.warn("Invalid toggle magnetometer packet: too short");
+				break;
+			}
+
+			const auto sensorId = m_Packet[12];
+			const auto enabled = m_Packet[13];
+
+			const auto sensor = sensorManager.getSensors().at(sensorId);
+			if (sensor == nullptr) {
+				m_Logger.warn("Invalid sensor ID in toggle magnetometer packet");
+				break;
+			} 
+
+			bool result = false;
+			if (sensor->supportsTogglingMagnetometer()) {
+				result = sensor->toggleMagnetometer(enabled);
+			} else {
+				m_Logger.warn("Received toggle magnetometer packet for sensor that doesn't support it");
+			}
+
+			sendToggleMagnetometerResult(sensorId, result);
+
 			break;
 	}
 }
diff --git a/src/network/connection.h b/src/network/connection.h
index 7b8f7b6a2..76bb3278a 100644
--- a/src/network/connection.h
+++ b/src/network/connection.h
@@ -158,6 +158,9 @@ class Connection {
 	// PACKET_SENSOR_INFO 15
 	void sendSensorInfo(Sensor* sensor);
 
+	// PACKET_TOGGLE_MAGNETOMETER 21
+	void sendToggleMagnetometerResult(uint8_t sensorId, bool result);
+
 	bool m_Connected = false;
 	SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection");
 
diff --git a/src/network/packets.h b/src/network/packets.h
index e962a6af0..d91e1da71 100644
--- a/src/network/packets.h
+++ b/src/network/packets.h
@@ -47,6 +47,7 @@
 #define PACKET_TEMPERATURE 20
 // #define PACKET_USER_ACTION 21 // Joycon buttons only currently
 #define PACKET_FEATURE_FLAGS 22
+#define PACKET_TOGGLE_MAGNETOMETER 23
 
 #define PACKET_BUNDLE 100