From f8b3bc1e47595e3b7b931544cdc9a9ad113e7d3e Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@rcor.de>
Date: Tue, 21 Nov 2023 21:34:07 +0100
Subject: [PATCH] Added formatted printing

---
 include/sta/debug/debug.hpp              | 17 +++++++++++++----
 include/sta/debug/printing/printable.hpp |  6 ++++++
 src/debug/printing/printable.cpp         | 15 ++++++++++++++-
 3 files changed, 33 insertions(+), 5 deletions(-)

diff --git a/include/sta/debug/debug.hpp b/include/sta/debug/debug.hpp
index 0396ea1..7a09609 100644
--- a/include/sta/debug/debug.hpp
+++ b/include/sta/debug/debug.hpp
@@ -19,7 +19,7 @@ namespace sta
  *
  * @ingroup sta_core_debug
  */
-#  define STA_DEBUG_PRINT(...)      sta::Debug->print(__VA_ARGS__)
+#  define STA_DEBUG_PRINT(...)      	sta::Debug->print(__VA_ARGS__)
 
 /**
  * @brief Debug print message followed by new-line to UART.
@@ -28,12 +28,21 @@ namespace sta
  *
  * @ingroup sta_core_debug
  */
-#  define STA_DEBUG_PRINTLN(...)    sta::Debug->println(__VA_ARGS__)
+#  define STA_DEBUG_PRINTLN(...)    	sta::Debug->println(__VA_ARGS__)
+
+/**
+ * @brief Formatted debug printing with new-line.
+ *
+ * @param fmt See @ref sta::PrintableUART::printf
+ * @param ... See @ref sta::PrintableUART::printf
+ */
+#  define STA_DEBUG_PRINTF(fmt, ...)	sta::Debug->printf(fmt, __VA_ARGS__)
 
 #else // !STA_DEBUGGING_ENABLED
 
-#  define STA_DEBUG_PRINT(...)      ((void)0)
-#  define STA_DEBUG_PRINTLN(...)    ((void)0)
+#  define STA_DEBUG_PRINT(...)      	((void)0)
+#  define STA_DEBUG_PRINTLN(...)    	((void)0)
+#  define STA_DEBUG_PRINTF(fmt, ...)	((void)0)
 
 #endif // STA_DEBUGGING_ENABLED
 
diff --git a/include/sta/debug/printing/printable.hpp b/include/sta/debug/printing/printable.hpp
index 77117a3..e1030fd 100644
--- a/include/sta/debug/printing/printable.hpp
+++ b/include/sta/debug/printing/printable.hpp
@@ -26,6 +26,12 @@ namespace sta
     class Printable
     {
     public:
+    	/**
+    	 * @brief fmt	The string defining the desired format.
+    	 * @breif ...	The parameters for the formatted string.
+    	 */
+    	void printf(const char * fmt, ...);
+
         /**
          * @brief Print single character.
          *
diff --git a/src/debug/printing/printable.cpp b/src/debug/printing/printable.cpp
index df6dcba..0c3bebd 100644
--- a/src/debug/printing/printable.cpp
+++ b/src/debug/printing/printable.cpp
@@ -3,12 +3,25 @@
 #include <cinttypes>
 #include <cstring>
 #include <cstdio>
+#include <cstdint>
+#include <stdarg.h>
 
 #include <sta/debug/assert.hpp>
 #include <sta/lang.hpp>
 
 namespace sta 
 {
+	void Printable::printf(const char * fmt, ...)
+	{
+		va_list args;
+		va_start (args, fmt);
+		char str[2*strlen(fmt)];
+		int n = vsnprintf(str, 2*strlen(fmt), fmt, args);
+		STA_ASSERT(n > 0);
+
+		println(str);
+	}
+
     void Printable::print(char c)
     {
         print(&c, 1);
@@ -182,4 +195,4 @@ namespace sta
         print(buffer, digits);
     }
 
-} // namespace sta
\ No newline at end of file
+} // namespace sta