From 596d3cf12ee23aec40a8f3614356b38ffa29fe5d Mon Sep 17 00:00:00 2001
From: Gergely Nagy <algernon@keyboard.io>
Date: Fri, 18 May 2018 18:06:46 +0200
Subject: [PATCH] Implement debouncing

This adds a very simple debouncing algorithm for debouncing. It works by making
sure a key is not changing state more than a configurable amount of cycles.
Defaults to five cycles for now.

Fixes #1.

Signed-off-by: Gergely Nagy <algernon@keyboard.io>
---
 src/kaleidoscope/hardware/ErgoDox.cpp | 49 +++++++++++++++++++++------
 src/kaleidoscope/hardware/ErgoDox.h   |  7 ++++
 2 files changed, 45 insertions(+), 11 deletions(-)

diff --git a/src/kaleidoscope/hardware/ErgoDox.cpp b/src/kaleidoscope/hardware/ErgoDox.cpp
index 2b2d0ed3..89ec936b 100644
--- a/src/kaleidoscope/hardware/ErgoDox.cpp
+++ b/src/kaleidoscope/hardware/ErgoDox.cpp
@@ -28,6 +28,8 @@ ErgoDoxScanner ErgoDox::scanner_;
 uint8_t ErgoDox::previousKeyState_[ROWS];
 uint8_t ErgoDox::keyState_[ROWS];
 uint8_t ErgoDox::masks_[ROWS];
+uint8_t ErgoDox::debounce_matrix_[ROWS][COLS];
+uint8_t ErgoDox::debounce = 5;
 
 void ErgoDox::setup(void) {
   wdt_disable();
@@ -53,20 +55,23 @@ void ErgoDox::setup(void) {
   setStatusLEDBrightness(3, 15);
 }
 
-void ErgoDox::readMatrix() {
-  for (uint8_t i = 0; i < ROWS / 2; i++) {
-    scanner_.selectRow(i);
-    scanner_.selectRow(i + ROWS / 2);
+void ErgoDox::readMatrixRow(uint8_t row) {
+  uint8_t mask, cols;
 
-    // TODO(algernon): debouncing
+  previousKeyState_[row] = keyState_[row];
+  mask = debounceMaskForRow(row);
+  cols = (scanner_.readCols(row) & mask) | (keyState_[row] & ~mask);
+  debounceRow(cols ^ keyState_[row], row);
+  keyState_[row] = cols;
+}
 
-    // left side
-    previousKeyState_[i] = keyState_[i];
-    keyState_[i] = scanner_.readCols(i);
+void ErgoDox::readMatrix() {
+  for (uint8_t row = 0; row < ROWS / 2; row++) {
+    scanner_.selectRow(row);
+    scanner_.selectRow(row + ROWS / 2);
 
-    // right side
-    previousKeyState_[i + ROWS / 2] = keyState_[i + ROWS / 2];
-    keyState_[i + ROWS / 2] = scanner_.readCols(i + ROWS / 2);
+    readMatrixRow(row);
+    readMatrixRow(row + ROWS / 2);
 
     scanner_.unselectRows();
   }
@@ -159,6 +164,28 @@ void ErgoDox::resetDevice() {
   asm volatile("jmp 0x7E00");
 }
 
+
+uint8_t ErgoDox::debounceMaskForRow(uint8_t row) {
+  uint8_t result = 0;
+
+  for (uint8_t c = 0; c < COLS; ++c) {
+    if (debounce_matrix_[row][c]) {
+      --debounce_matrix_[row][c];
+    } else {
+      result |= (1 << c);
+    }
+  }
+  return result;
+}
+
+void ErgoDox::debounceRow(uint8_t change, uint8_t row) {
+  for (uint8_t i = 0; i < COLS; ++i) {
+    if (change & (1 << i)) {
+      debounce_matrix_[row][i] = debounce;
+    }
+  }
+}
+
 }
 }
 
diff --git a/src/kaleidoscope/hardware/ErgoDox.h b/src/kaleidoscope/hardware/ErgoDox.h
index 6b7500ed..42a65df4 100644
--- a/src/kaleidoscope/hardware/ErgoDox.h
+++ b/src/kaleidoscope/hardware/ErgoDox.h
@@ -75,11 +75,18 @@ class ErgoDox {
 
   void resetDevice();
 
+  static uint8_t debounce;
+
  private:
   static ErgoDoxScanner scanner_;
   static uint8_t previousKeyState_[ROWS];
   static uint8_t keyState_[ROWS];
   static uint8_t masks_[ROWS];
+  static uint8_t debounce_matrix_[ROWS][COLS];
+
+  static uint8_t debounceMaskForRow(uint8_t row);
+  static void debounceRow(uint8_t change, uint8_t row);
+  static void readMatrixRow(uint8_t row);
 };
 
 #define KEYMAP_STACKED(                                         \