#include <Keypad.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Definieer de toetsen op het toetsenbord
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {9, 8, 7, 13};
byte colPins[COLS] = {12, 4, 3, 2};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);
String command;

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Voer commando in");
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    lcd.init();
    lcd.setCursor(0, 0);
    lcd.setCursor(0, 1);
    lcd.print("Commando " + String(command + key));

    if (key == '*') {
      // Reset het commando als * wordt ingedrukt
      command = "";
      lcd.init();
      lcd.setCursor(0, 0);
      lcd.setCursor(0, 1);
      lcd.print("Commando ");
    } else if (key == '#') {
      // Voer het commando uit op de geselecteerde servo
      int targetAngle = command.toInt();
      command = ""; // Leeg het commando
    } else if (key == 'A' || key == 'B' || key == 'C' || key == 'D') {
      lcd.init();
      lcd.setCursor(0, 0);
      lcd.setCursor(0, 1);
      lcd.print("Commando " + String(command));
    } else {
      // Voeg het cijfer toe aan het commando
      command += key;
    }
  }
}