Added layer switching and debouncing

1 files changed, 178 insertions, 29 deletions

diff --git a/src/hid.c b/src/hid.c
index 678e744..a406a2c 100644
--- a/src/hid.c
+++ b/src/hid.c
@@ -1,56 +1,201 @@
 #include "hid.h"
+#include "keycode.h"
 #include "split.h"
 #include "keymat.h"
 #include "keysym.h"
 #include "keymap.h"
 
+#include "hardware/timer.h"
 #include "bsp/board.h"
 #include "pico/types.h"
 
+#include <string.h>
+
+struct layerkey {
+	uint layer;
+	uint key;
+};
+
+static uint32_t keysyms[KEY_ROWS][KEY_COLS] = { 0 };
+
+static struct layerkey active_stack[16] = { 0 };
+static uint active_top = 0;
+
+static uint8_t hid_report_prev[6] = { 0 };
+static uint8_t hid_report[6] = { 0 };
+static uint hid_report_len = 0;
+
+static uint64_t bounce_mat[KEY_ROWS][KEY_COLS] = { 0 };
+
+static bool seen_mat[KEY_ROWS][KEY_COLS];
+
+static void active_pop(uint layer);
+static void active_push(uint layer, uint key);
+
 void
-hid_init(void)
+active_pop(uint layer)
+{
+	uint i;
+
+	for (i = layer + 1; i <= active_top; i++)
+		active_stack[i-1] = active_stack[i];
+	if (layer <= active_top)
+		active_top--;
+}
+
+void
+active_push(uint layer, uint key)
+{
+	if (active_top == ARRLEN(active_stack) - 1) {
+		WARN("Active stack overflow");
+		return;
+	}
+	active_top += 1;
+	active_stack[active_top].layer = layer;
+	active_stack[active_top].key = key;
+}
+
+void
+add_keycode(uint8_t keycode)
+{
+	if (hid_report_len >= 6) {
+		WARN("HID report overflow");
+		return;
+	}
+
+	hid_report[hid_report_len] = keycode;
+	hid_report_len++;
+}
+
+void
+handle_keypress(uint x, uint y)
+{
+	uint32_t ksym;
+	int i;
+
+	if (!keymat_prev[y][x]) {
+		for (i = (int) active_top; i >= 0; i--) {
+			ksym = keymap_layers[active_stack[i].layer][y][x];
+			if (ksym == KC_NO) return;
+			if (ksym != KC_TRNS)
+				break;
+		}
+		if (i < 0) return;
+		keysyms[y][x] = ksym;
+
+		if (IS_SWITCH(keysyms[y][x])) {
+			INFO("LAYER %u", TO_LAYER(keysyms[y][x]));
+			active_push(TO_LAYER(ksym), y * KEY_COLS + x);
+			for (i = 0; i <= (int) active_top; i++) {
+				INFO("%i. ACTIVE %u %u", i,
+					active_stack[i].layer, active_stack[i].key);
+			}
+		}
+	}
+
+	if (!seen_mat[y][x]) {
+		if (IS_CTRL(keysyms[y][x])) {
+			if (IS_RIGHT(keysyms[y][x])) {
+				add_keycode(KC_RIGHT_CTRL);
+			} else {
+				add_keycode(KC_LEFT_CTRL);
+			}
+		}
+
+		if (IS_SHIFT(keysyms[y][x])) {
+			if (IS_RIGHT(keysyms[y][x])) {
+				add_keycode(KC_RIGHT_SHIFT);
+			} else {
+				add_keycode(KC_LEFT_SHIFT);
+			}
+		}
+
+		if (IS_ALT(keysyms[y][x])) {
+			if (IS_RIGHT(keysyms[y][x])) {
+				add_keycode(KC_RIGHT_ALT);
+			} else {
+				add_keycode(KC_LEFT_ALT);
+			}
+		}
+
+		if (IS_GUI(keysyms[y][x])) {
+			if (IS_RIGHT(keysyms[y][x])) {
+				add_keycode(KC_RIGHT_GUI);
+			} else {
+				add_keycode(KC_LEFT_GUI);
+			}
+		}
+
+		if (IS_CODE(keysyms[y][x])) {
+			add_keycode(TO_CODE(keysyms[y][x]));
+			INFO("CODE %u %u", active_top, keysyms[y][x]);
+		}
+
+		seen_mat[y][x] = true;
+	}
+}
+
+void
+handle_keyrelease(uint x, uint y)
 {
-	
+	uint i;
+
+	if (keymat_prev[y][x]) {
+		for (i = 1; i <= active_top; i++) {
+			if (active_stack[i].key == y * KEY_COLS + x) {
+				active_pop(i);
+				break;
+			}
+		}
+	}
 }
 
 bool
-hid_gen_report(uint8_t *report)
+update_report(void)
 {
-	int keycnt;
-	uint x, y, p;
+	uint64_t now_us;
+	uint keycnt;
+	uint x, y;
 
 	keycnt = 0;
-	for (y = 0; y < KEY_ROWS * 2; y++) {
+	now_us = time_us_64();
+	for (y = 0; y < KEY_ROWS; y++) {
 		for (x = 0; x < KEY_COLS; x++) {
-			if (!keymat[y][x])
-				continue;
-			if (keycnt >= 6) break;
-			DEBUG("PRESS %i %", x, y);
-			p = y * KEY_COLS + x;
-			report[keycnt] = TO_CODE(keymap_layers[0][p]);
-			keycnt++;
+			if (keymat[y][x] != keymat_prev[y][x]) {
+				if (bounce_mat[y][x] > now_us - 25000) {
+					WARN("Bouncing prevented %i vs %i",
+						keymat[y][x], keymat_prev[y][x]);
+					keymat[y][x] = keymat_prev[y][x];
+				} else {
+					bounce_mat[y][x] = now_us;
+				}
+			}
+
+			if (keymat[y][x]) {
+				handle_keypress(x, y);
+			} else {
+				handle_keyrelease(x, y);
+			}
 		}
 	}
 
 	return keycnt > 0;
 }
 
+void
+hid_init(void)
+{
+}
+
 bool
 send_keyboard_report(void)
 {
-	static bool cleared = true;
-	uint8_t report[6] = { 0 };
-	bool any;
-
-	any = hid_gen_report(report);
-
-	if (any) {
-		tud_hid_keyboard_report(REPORT_ID_KEYBOARD, 0, report);
-		cleared = false;
-		return true;
-	} else if (!cleared) {
-		tud_hid_keyboard_report(REPORT_ID_KEYBOARD, 0, NULL);
-		cleared = true;
+	uint i;
+	if (memcmp(hid_report, hid_report_prev, sizeof(hid_report))) {
+		for (i = 0; i < 6; i++)
+			INFO("REPORT %u: %u", i, hid_report[i]);
+		tud_hid_keyboard_report(REPORT_ID_KEYBOARD, 0, hid_report);
+		memcpy(hid_report_prev, hid_report, sizeof(hid_report));
 		return true;
 	}
 
@@ -92,8 +237,6 @@ send_consumer_control_report(bool state)
 bool
 send_hid_report(int id)
 {
-	if (!tud_hid_ready()) return false;
-
 	switch (id) {
 	case REPORT_ID_KEYBOARD:
 		return send_keyboard_report();
@@ -121,5 +264,11 @@ tud_hid_report_complete_cb(uint8_t instance,
 void
 hid_task(void)
 {
-	send_hid_report(REPORT_ID_MIN);
+	update_report();
+	if (tud_hid_ready()) {
+		send_hid_report(REPORT_ID_MIN);
+		memset(hid_report, 0, sizeof(hid_report));
+		memset(seen_mat, 0, sizeof(seen_mat));
+		hid_report_len = 0;
+	}
 }