From 57b860ede743434792e5ae5b190fdeea906385b0 Mon Sep 17 00:00:00 2001
From: Gabriel Zerbib <moddingear@gmail.com>
Date: Wed, 10 Jun 2026 21:54:22 +0200
Subject: [PATCH 1/2] fix SIMPLEFOC_DISABLE_DEBUG, so that it works even
 without using the macro

---
 src/communication/SimpleFOCDebug.h | 33 ++++++++++++++++++++++++------
 1 file changed, 27 insertions(+), 6 deletions(-)

diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h
index c63b77af..fa3f0fb4 100644
--- a/src/communication/SimpleFOCDebug.h
+++ b/src/communication/SimpleFOCDebug.h
@@ -34,7 +34,33 @@
 
 // #define SIMPLEFOC_DISABLE_DEBUG
 
-#ifndef SIMPLEFOC_DISABLE_DEBUG 
+#ifdef SIMPLEFOC_DISABLE_DEBUG 
+class SimpleFOCDebug {
+public:
+    inline static void enable(Print* debugPrint){}
+
+    inline static void println(const __FlashStringHelper* msg){}
+    inline static void println(const StringSumHelper msg){}
+    inline static void println(const char* msg){}
+    inline static void println(const __FlashStringHelper* msg, float val){}
+    inline static void println(const char* msg, float val){}
+    inline static void println(const __FlashStringHelper* msg, int val){}
+    inline static void println(const char* msg, int val){}
+    inline static void println(const char* msg, char val){}
+    inline static void println(){}
+    inline static void println(int val){}
+    inline static void println(float val){}
+
+    inline static void print(const char* msg){}
+    inline static void print(const __FlashStringHelper* msg){}
+    inline static void print(const StringSumHelper msg){}
+    inline static void print(int val){}
+    inline static void print(float val){}
+};
+
+#define SIMPLEFOC_DEBUG(msg, ...)
+
+#else
 
 class SimpleFOCDebug {
 public:
@@ -66,11 +92,6 @@ class SimpleFOCDebug {
 #define SIMPLEFOC_DEBUG(msg, ...) \
     SimpleFOCDebug::println(F(msg), ##__VA_ARGS__)
 
-#else  //ifndef SIMPLEFOC_DISABLE_DEBUG
- 
-#define SIMPLEFOC_DEBUG(msg, ...)
-
-
 #endif //ifndef SIMPLEFOC_DISABLE_DEBUG
 #endif
 

From 4494158a1cdac23d20f13a92959d6f9269bfce83 Mon Sep 17 00:00:00 2001
From: Gabriel Zerbib <moddingear@gmail.com>
Date: Sat, 13 Jun 2026 19:35:12 +0200
Subject: [PATCH 2/2] Add printf to SimpleFOCDebug

---
 src/communication/SimpleFOCDebug.cpp | 10 ++++++++++
 src/communication/SimpleFOCDebug.h   |  4 ++++
 2 files changed, 14 insertions(+)

diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp
index 4d50b87c..f7035f0a 100644
--- a/src/communication/SimpleFOCDebug.cpp
+++ b/src/communication/SimpleFOCDebug.cpp
@@ -122,4 +122,14 @@ void SimpleFOCDebug::println() {
     }
 }
 
+void SimpleFOCDebug::printf(const char* msg, ...)
+{
+    va_list ap;
+    va_start(ap, msg);
+    if (_debugPrint != NULL) {
+        _debugPrint->vprintf(msg, ap);
+    }
+    va_end(ap);
+}
+
 #endif
\ No newline at end of file
diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h
index fa3f0fb4..cd20b9f5 100644
--- a/src/communication/SimpleFOCDebug.h
+++ b/src/communication/SimpleFOCDebug.h
@@ -56,6 +56,8 @@ class SimpleFOCDebug {
     inline static void print(const StringSumHelper msg){}
     inline static void print(int val){}
     inline static void print(float val){}
+
+    static void printf(const char* msg, ...){}
 };
 
 #define SIMPLEFOC_DEBUG(msg, ...)
@@ -84,6 +86,8 @@ class SimpleFOCDebug {
     static void print(int val);
     static void print(float val);
 
+    static void printf(const char* msg, ...);
+
 protected:
     static Print* _debugPrint;
 };