#include <Wire.h>

class KeypadHandler {
public:
  static constexpr int MAXSIZE = 4;
  KeypadHandler() {}
  KeypadHandler(int rowPins[MAXSIZE], int colPins[MAXSIZE]) {
    memcpy(this->rowPins, rowPins, sizeof(int) * MAXSIZE);
    memcpy(this->colPins, colPins, sizeof(int) * MAXSIZE);
    for (int i = 0; i < MAXSIZE; i++){
      pinMode(rowPins[i], OUTPUT);
    }
    for (int i = 0; i < MAXSIZE; i++) {
      pinMode(colPins[i], INPUT_PULLUP);
    }
  }

  char getKey() const noexcept {
    for (int row = 0; row < MAXSIZE; row++) {
    digitalWrite(rowPins[row], LOW);
    for (int col = 0; col < MAXSIZE; col++) {
      bool noVoltage = true;
      noVoltage &= (digitalRead(colPins[col]) == LOW);
        if (!noVoltage) {
          continue;
        }
        noVoltage &= (digitalRead(colPins[col]) == LOW);
        while (digitalRead(colPins[col]) == LOW);
        if (noVoltage) {
          digitalWrite(rowPins[row], HIGH);
          const int index = row * MAXSIZE + col;
          return symbols[index];
        }
      }
      digitalWrite(rowPins[row], HIGH);
    }
    return 0;
  }
private:
  static constexpr char *symbols = "123A456B789C*0#D";
  int rowPins[MAXSIZE], colPins[MAXSIZE];
};

KeypadHandler kph;
void setup() {
  int rowPins[KeypadHandler::MAXSIZE] = {2, 3, 4, 5};
  int colPins[KeypadHandler::MAXSIZE] = {6, 7, 8, 9};
  kph = KeypadHandler(rowPins, colPins);
  Serial.begin(9600);
}

void loop() {
  char key = kph.getKey();
  if (key) {
    Serial.println(key);
  }
}