From 8fc7eb7727b48b92bd2683500ca955dc456b3db2 Mon Sep 17 00:00:00 2001
From: Jesse Vincent <jesse@keyboard.io>
Date: Mon, 28 Nov 2016 18:54:16 -0800
Subject: [PATCH] Test mode: speed up color cycling and implement matrix
 testing

---
 src/TestMode.cpp | 69 ++++++++++++++++++++++++++++++++++++++++--------
 src/TestMode.h   |  3 ++-
 2 files changed, 60 insertions(+), 12 deletions(-)

diff --git a/src/TestMode.cpp b/src/TestMode.cpp
index 1d1c34ad..e94c50bc 100644
--- a/src/TestMode.cpp
+++ b/src/TestMode.cpp
@@ -4,6 +4,11 @@
 
 static LEDRainbowEffect testRainbowEffect;
 
+cRGB red;
+cRGB blue;
+
+#define LED_TEST_DELAY 2000
+
 TestMode_::TestMode_(void) {
 }
 
@@ -11,40 +16,39 @@ void TestMode_::TestLEDs(void) {
     // make all LEDs dim red
     LEDControl.set_all_leds_to(50,0,0);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all LEDs dim blue
     LEDControl.set_all_leds_to(0,50,0);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all LEDs dim green
     LEDControl.set_all_leds_to(0,0,50);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all LEDs dim white
     LEDControl.set_all_leds_to(50,50,50);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all the LEDs bright red
     LEDControl.set_all_leds_to(200,0,0);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all the LEDs bright green
     LEDControl.set_all_leds_to(0,200,0);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all the LEDs bright blue
     LEDControl.set_all_leds_to(0,0,200);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // make all the LEDs bright white (1.6A)
     LEDControl.set_all_leds_to(160,160,160);
     led_sync();
-    delay(1000);
+    delay(LED_TEST_DELAY);
     // rainbow for 10 seconds
-    for(auto i=0; i<10000; i++ ) {
+    for(auto i=0; i<1000; i++ ) {
         testRainbowEffect.update();
         led_sync();
-        delay(1);
     }
     // set all the keys to red
     LEDControl.set_all_leds_to(50,0,0);
@@ -56,14 +60,57 @@ void TestMode_::TestLEDs(void) {
 
 
 
+
 void TestMode_::TestMatrix () {
-    KeyboardHardware.read_matrix();
+   while(1) {
+   	 KeyboardHardware.read_matrix();
+    for (byte row = 0; row < 4; row++) {
+        for (byte col = 0; col < 8; col++) {
+
+            uint8_t keynum = (row*8)+(col);
+
+            uint8_t keyState = (bitRead(KeyboardHardware.previousLeftHandState.all, keynum) << 0) |
+                               (bitRead(KeyboardHardware.leftHandState.all,         keynum) << 1);
+
+		if(keyState ==3 ) {
+		Serial.print(" Key: ");
+		Serial.print(keynum);
+		Serial.print(" value ");
+		Serial.println(keyState);
+	    		KeyboardHardware.led_set_crgb_at(row, 7-col, red);
+		} else if (keyState ==1) {
+	    		KeyboardHardware.led_set_crgb_at(row,7-col, blue);
+		}
 
+            keyState = (bitRead(KeyboardHardware.previousRightHandState.all, keynum) << 0) |
+              	       (bitRead(KeyboardHardware.rightHandState.all,         keynum) << 1);
+
+		if(keyState ==3 ) {
+		Serial.print(" Key: ");
+		Serial.print(keynum);
+		Serial.print(" value ");
+		Serial.println(keyState);
+	    		KeyboardHardware.led_set_crgb_at(row, 15-col, red);
+		} else if (keyState ==1) {
+	    		KeyboardHardware.led_set_crgb_at(row,15-col, blue);
+		}
+        }
+    }
+	KeyboardHardware.led_sync();
+   }
 }
 
 void TestMode_::setup() {
+red.r=101;
+blue.b=101;
+//	TestLEDs();
     eventHandlers[0] = handle_key_event_test;
 }
+
+void TestMode_::loop() {
+	TestMatrix();
+}
+
 bool handle_key_event_test(Key mappedKey, byte row, byte col, uint8_t keyState) {
     Serial.write(row);
     return false;
diff --git a/src/TestMode.h b/src/TestMode.h
index c25f6f3e..7455e485 100644
--- a/src/TestMode.h
+++ b/src/TestMode.h
@@ -5,12 +5,13 @@ class TestMode_ {
   public:
     TestMode_(void);
     void setup();
+    void loop();
 
+  private:
     void TestLEDs();
     void TestKeys();
     void TestMatrix();
     void TestOneKey();
-  private:
 };
 
 extern TestMode_ TestMode;