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

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servoA; // Hand (open/dicht)
Servo servoB; // Vooruit/achteruit
Servo servoC; // Omhoog/omlaag
Servo servoD; // Draaien (links/rechts)

// 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;
char selectedServo = 'A'; // Het geselecteerde servo (A, B, C of D)
int angle = 0; // Hoekinstelling

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

  servoA.attach(5);
  servoB.attach(6);
  servoC.attach(10);
  servoD.attach(11);

  // Stel startposities in voor de servo's
  servoA.write(80);
  servoB.write(30);
  servoC.write(120);
  servoD.write(90);
}

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

  if (key) {
    lcd.init();
    lcd.setCursor(0, 0);
    lcd.print("Servo " + String(selectedServo));
    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.print("Servo " + String(selectedServo));
      lcd.setCursor(0, 1);
      lcd.print("Commando ");
    } else if (key == '#') {
      // Voer het commando uit op de geselecteerde servo
      int targetAngle = command.toInt();
      if (selectedServo == 'A') {
        moveServo(servoA, targetAngle);
      } else if (selectedServo == 'B') {
        moveServo(servoB, targetAngle);
      } else if (selectedServo == 'C') {
        moveServo(servoC, targetAngle);
      } else if (selectedServo == 'D') {
        moveServo(servoD, targetAngle);
      }
      command = ""; // Leeg het commando
    } else if (key == 'A' || key == 'B' || key == 'C' || key == 'D') {
      // Selecteer een servo (A, B, C of D)
      selectedServo = key;
      lcd.init();
      lcd.setCursor(0, 0);
      lcd.print("Servo " + String(selectedServo));
      lcd.setCursor(0, 1);
      lcd.print("Commando " + String(command));
    } else {
      // Voeg het cijfer toe aan het commando
      command += key;
    }
  }
}

// Functie om de servo naar de gewenste hoek te bewegen
void moveServo(Servo servo, int targetAngle) {
  int currentAngle = servo.read();
  if (targetAngle >= currentAngle) {
    for (int a = currentAngle; a <= targetAngle; a++) {
      servo.write(a);
      delay(10);
    }
  } else {
    for (int a = currentAngle; a >= targetAngle; a--) {
      servo.write(a);
      delay(10);
    }
  }
}