#include "Wire.h"
#include "I2CKeyPad.h"
const uint8_t KEYPAD_ADDRESS = 0x20;

// P7 C4
// P6 C3
// P5 C2
// P4 C1
// P3 R4
// P2 R3
// P1 R2
// P0 R1

I2CKeyPad keyPad(KEYPAD_ADDRESS);

uint32_t start, stop;
uint32_t lastKeyPressed = 0;

void setup() {
  Serial.begin(115200);

  Wire.begin();
  Wire.setClock(400000);

  if (keyPad.begin() == false) {
    Serial.println("\nERROR: cannot communicate to keypad.\nPlease reboot.\n");
    while(1);
  }
}

void loop() {
  uint32_t now = millis();
  char keys[] = "123A456B789C*0#DNF";  //  N = NoKey, F = Fail

  if (now - lastKeyPressed >= 500) {
    lastKeyPressed = now;

    start = micros();
    uint8_t index = keyPad.getKey();
    stop = micros();

    Serial.print(millis());
    Serial.print("\t");
    Serial.print(index);
    Serial.print("\t");
    Serial.print(keys[index]);
    Serial.print("\t");
    Serial.println(stop - start);
  }
}