From 366f6c9276d32c6961cbce4abd60c8ca06344194 Mon Sep 17 00:00:00 2001
From: Jacob Alexander <haata@kiibohd.com>
Date: Sat, 26 Apr 2014 01:29:09 -0700
Subject: [PATCH] Kishsaver is fully working with DPH!

- More keyboard layouts and enhancements to come :D
---
 Macro/PartialMap/macro.c | 41 ++++++++++++++++++++++++++++++++++++++--
 Macro/PartialMap/macro.h |  1 +
 Scan/DPH/scan_loop.c     | 24 +++++------------------
 3 files changed, 45 insertions(+), 21 deletions(-)

diff --git a/Macro/PartialMap/macro.c b/Macro/PartialMap/macro.c
index a55a5cb..80f0c5d 100644
--- a/Macro/PartialMap/macro.c
+++ b/Macro/PartialMap/macro.c
@@ -79,10 +79,45 @@ inline void Macro_bufferAdd( uint8_t byte )
 	// Default function for adding keys to the KeyIndex_Buffer, does a DefaultMap_Lookup
 	if ( KeyIndex_BufferUsed < KEYBOARD_BUFFER )
 	{
-		KeyIndex_Buffer[KeyIndex_BufferUsed++] = DefaultMap_Lookup[byte];
+		uint8_t key = DefaultMap_Lookup[byte];
+		for ( uint8_t c = 0; c < KeyIndex_BufferUsed; c++ )
+		{
+			// Key already in the buffer
+			if ( KeyIndex_Buffer[c] == key )
+				return;
+		}
+
+		// Add to the buffer
+		KeyIndex_Buffer[KeyIndex_BufferUsed++] = key;
 	}
 }
 
+inline void Macro_bufferRemove( uint8_t byte )
+{
+	uint8_t key = DefaultMap_Lookup[byte];
+
+	// Check for the released key, and shift the other keys lower on the buffer
+	for ( uint8_t c = 0; c < KeyIndex_BufferUsed; c++ )
+	{
+		// Key to release found
+		if ( KeyIndex_Buffer[c] == key )
+		{
+			// Shift keys from c position
+			for ( uint8_t k = c; k < KeyIndex_BufferUsed - 1; k++ )
+				KeyIndex_Buffer[k] = KeyIndex_Buffer[k + 1];
+
+			// Decrement Buffer
+			KeyIndex_BufferUsed--;
+
+			return;
+		}
+	}
+
+	// Error case (no key to release)
+	erro_msg("Could not find key to release: ");
+	printHex( key );
+}
+
 inline void Macro_finishWithUSBBuffer( uint8_t sentKeys )
 {
 }
@@ -96,11 +131,13 @@ inline void Macro_process()
 	// Loop through input buffer
 	for ( uint8_t index = 0; index < KeyIndex_BufferUsed; index++ )
 	{
+		//print(" KEYS: ");
+		//printInt8( KeyIndex_BufferUsed );
 		// Get the keycode from the buffer
 		uint8_t key = KeyIndex_Buffer[index];
 
 		// Set the modifier bit if this key is a modifier
-		if ( key & KEY_LCTRL ) // AND with 0xE0
+		if ( (key & KEY_LCTRL) == KEY_LCTRL ) // AND with 0xE0
 		{
 			USBKeys_Modifiers |= 1 << (key ^ KEY_LCTRL); // Left shift 1 by key XOR 0xE0
 
diff --git a/Macro/PartialMap/macro.h b/Macro/PartialMap/macro.h
index b089f48..1455200 100644
--- a/Macro/PartialMap/macro.h
+++ b/Macro/PartialMap/macro.h
@@ -40,6 +40,7 @@
 // ----- Functions -----
 
 void Macro_bufferAdd( uint8_t byte );
+void Macro_bufferRemove( uint8_t byte );
 void Macro_finishWithUSBBuffer( uint8_t sentKeys );
 void Macro_process();
 void Macro_setup();
diff --git a/Scan/DPH/scan_loop.c b/Scan/DPH/scan_loop.c
index 74032e0..c4d4284 100644
--- a/Scan/DPH/scan_loop.c
+++ b/Scan/DPH/scan_loop.c
@@ -324,8 +324,6 @@ inline uint8_t Scan_loop()
 // NOTE: Only really required for implementing "tricks" in converters for odd protocols
 void Scan_finishedWithBuffer( uint8_t sentKeys )
 {
-	// Convenient place to clear the KeyIndex_Buffer
-	KeyIndex_BufferUsed = 0;
 	return;
 }
 
@@ -821,7 +819,7 @@ void testColumn( uint8_t strobe )
 				// Debug message
 				// <key> [<strobe>:<mux>] : <sense val> : <delta + threshold> : <margin>
 				dbug_msg("0x");
-				printHex_op( key, 2 );
+				printHex_op( key, 1 );
 				print(" [");
 				printInt8( strobe );
 				print(":");
@@ -843,22 +841,10 @@ void testColumn( uint8_t strobe )
 		else
 		{
 			// If the key was previously pressed, remove from the buffer
-			for ( uint8_t c = 0; c < KeyIndex_BufferUsed; c++ )
-                        {
-                                // Key to release found
-                                if ( KeyIndex_Buffer[c] == key )
-                                {
-                                        // Shift keys from c position
-                                        for ( uint8_t k = c; k < KeyIndex_BufferUsed - 1; k++ )
-                                                KeyIndex_Buffer[k] = KeyIndex_Buffer[k + 1];
-
-                                        // Decrement Buffer
-                                        KeyIndex_BufferUsed--;
-
-                                        break;
-                                }
-                        }
-
+			if ( KeyIndex_BufferUsed > 0 && keys_debounce[key] >= DEBOUNCE_THRESHOLD )
+			{
+				Macro_bufferRemove( key );
+			}
 
 			// Clear debounce entry
 			keys_debounce[key] = 0;