From 27407f36bb83d3ccc7aec9c2934b044735c651b1 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Sat, 29 Jun 2024 17:30:11 +0200
Subject: [PATCH] Updated spatz implementation

---
 include/sta/debug/printing/printable.hpp |  9 -----
 include/sta/debug/spatz.hpp              | 14 ++++++--
 src/debug/spatz.cpp                      | 43 ++++++++++++++++++++----
 3 files changed, 48 insertions(+), 18 deletions(-)

diff --git a/include/sta/debug/printing/printable.hpp b/include/sta/debug/printing/printable.hpp
index c913bff..d453702 100644
--- a/include/sta/debug/printing/printable.hpp
+++ b/include/sta/debug/printing/printable.hpp
@@ -201,15 +201,6 @@ namespace sta
          * @param length    Buffer length
          */
         void read(double * buffer, size_t length);
-    public:
-        virtual void request(uint8_t * txBuffer, size_t txLength, uint8_t * rxBuffer, size_t rxLength) = 0;
-
-        void request(uint8_t * txBuffer, size_t txLength, float * rxBuffer, size_t rxLength);
-
-        void request(uint8_t * txBuffer, size_t txLength, double * rxBuffer, size_t rxLenght);
-
-        
-
     private:
         /**
          * @brief Print unsigned integer in selected base.
diff --git a/include/sta/debug/spatz.hpp b/include/sta/debug/spatz.hpp
index 5f6f54f..7b69f4c 100644
--- a/include/sta/debug/spatz.hpp
+++ b/include/sta/debug/spatz.hpp
@@ -1,6 +1,8 @@
 #ifndef STA_CORE_SPATZ_HPP
 #define STA_CORE_SPATZ_HPP
 
+#include <sta/mutex.hpp>
+
 #include <sta/config.hpp>
 #ifdef STA_SPATZ_ENABLED
 
@@ -9,8 +11,6 @@
 #   error "Cannot use SPATZ without debugging being enabled."
 #endif // STA_DEBUGGING_ENABLED
 
-#include <sta/mutex.hpp>
-
 namespace sta
 {
     namespace spatz
@@ -51,6 +51,16 @@ namespace sta
     } // namespace spatz
 } // namespace sta
 
+#else
+
+namespace sta
+{
+    namespace spatz
+    {
+        void init(Mutex * mutex);
+    } // namespace spatz
+} // namespace sta
+
 #endif // STA_SPATZ_ENABLED
 
 #endif // STA_CORE_SPATZ_HPP
diff --git a/src/debug/spatz.cpp b/src/debug/spatz.cpp
index 0e3082a..2f22c0b 100644
--- a/src/debug/spatz.cpp
+++ b/src/debug/spatz.cpp
@@ -2,6 +2,10 @@
 
 #ifdef STA_SPATZ_ENABLED
 
+#ifndef STA_INIT_SPATZ_ID
+#	error "The ID used to communicate SPATZ initialization was not defined."
+#endif // STA_INIT_SPATZ_ID
+
 namespace sta
 {
     namespace spatz
@@ -11,32 +15,57 @@ namespace sta
         void init(sta::Mutex * mutex)
         {
             mutex_ = mutex;
+            uint8_t msg = 0xFF;
+
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
+
+            // Wait until SPATZ sends the init byte.
+            while (msg != STA_INIT_SPATZ_ID)
+            {
+            	Debug->read(&msg, 1);
+            }
         }
 
         void request(uint8_t id, uint8_t * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            Debug->println("%d", id);
             Debug->read(buffer, length);
         }
 
         void request(uint8_t id, float * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            // Debug->println(id, IntegerBase::DEC);
+
+            Debug->printf("%d", id);
             Debug->read(buffer, length);
         }
 
         void request(uint8_t id, double * buffer, size_t length)
         {
-            sta::lock_guard lock(*mutex_);
+            sta::lock_guard<sta::Mutex> lock(*mutex_);
 
-            Debug->println(id);
+            Debug->println("%d", id);
             Debug->read(buffer, length);
         }
     } // namespace spatz    
 } // namespace sta
 
-#endif // STA_SPATZ_ENABLED
\ No newline at end of file
+#else
+
+namespace sta
+{
+	namespace spatz
+	{
+		void init(sta::Mutex * mutex)
+	    {
+	        // Nothing to do here.
+	    }
+	}  // namespace spatz
+} // namespace sta
+
+
+#endif // STA_SPATZ_ENABLED