From df18caf2ef56de16e7cba3b74be432e72caa45be Mon Sep 17 00:00:00 2001
From: MCUdude <hansibull@gmail.com>
Date: Fri, 15 Mar 2024 14:25:46 +0100
Subject: [PATCH] Fix bug in mux function

---
 src/SigmaDSP.cpp | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/SigmaDSP.cpp b/src/SigmaDSP.cpp
index 9d649a2..a6e86d1 100644
--- a/src/SigmaDSP.cpp
+++ b/src/SigmaDSP.cpp
@@ -90,7 +90,18 @@ void SigmaDSP::mux(uint16_t startMemoryAddress, uint8_t index, uint8_t numberOfI
   if(numberOfIndexes == 0)
     safeload_write(startMemoryAddress, index);
   else
-    demux(startMemoryAddress, index, numberOfIndexes);
+  {
+    uint8_t i = 0;
+    // Load leading zeros
+    for(; i < index; i++)
+      safeload_writeRegister(startMemoryAddress++, 0x00, false);
+    // Load index
+    i++;
+    safeload_writeRegister(startMemoryAddress++, 0x00800000, (i == numberOfIndexes ? true : false));
+    // Load tailing zeros
+    for(; i < numberOfIndexes; i++)
+      safeload_writeRegister(startMemoryAddress++, 0x00, (i == numberOfIndexes-1 ? true : false));
+  }
 }