#include "hardware.h"

uint8_t oldButtons = 0;
uint16_t oldKeys = 0;
uint16_t oldUtil = 0;
bool plotCap = false; 

void handleTestMode(state_t &state) {
    uint8_t buttons = buttonState();
    if (buttons != oldButtons) {
        oldButtons = buttons;
        Serial.print("Buttons:");
        Serial.println(buttons, HEX);
    }
/*
    Serial.print(">KnobE1aPin:");
    Serial.println(digitalRead(e1aPin));
    Serial.print(">KnobE2aPin:");
    Serial.println(digitalRead(e2aPin));
    Serial.print(">KnobE3aPin:");
    Serial.println(digitalRead(e3aPin));
    Serial.print(">KnobE4aPin:");
    Serial.println(digitalRead(e4aPin));

    Serial.print(">KnobE1bPin:");
    Serial.println(digitalRead(e1bPin));
    Serial.print(">KnobE2bPin:");
    Serial.println(digitalRead(e2bPin));
    Serial.print(">KnobE3bPin:");
    Serial.println(digitalRead(e3bPin));
    Serial.print(">KnobE4bPin:");
    Serial.println(digitalRead(e4bPin));
*/
    for (int i = 0; i < 4; i++) {
        int k = readKnob(i);
        if (k != 0) {
            Serial.print("Knob");
            Serial.print(i);
            Serial.print(":");
            Serial.println(k);
        }
    }

    uint16_t keys = keysTouched();
    if (keys != oldKeys) {
        Serial.print("Keys:");
        Serial.println(keys, HEX);
        oldKeys = keys;
    }

    if (buttons == 0x01) { 
        plotCap = !plotCap;
    }

    if (buttons == 0x08) { 
        state.instrument->mode = MODE_NORMAL;
    }

    if (plotCap) {
        for (int i = 0; i < 7; i++) {
            Serial.print(">control");
            Serial.print(i);
            Serial.print(":");
            Serial.println(readRawControl(i));
        }
    }

    delay(2);
}