#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x3F, 16, 2);
#include <SoftwareSerial.h>
SoftwareSerial btSerial(10, 11);

String btData;
int taste;

void setup() {
  lcd.init();                      // Display starten
  lcd.backlight();                 // Hintergrundlicht am Display
  btSerial.begin(9600);
  Serial.begin(115200);
  pinMode(3, OUTPUT); // PWM_A-Signal als Ausgang
  pinMode(2, OUTPUT); // AIN_1 digitaler Ausgang
  pinMode(4, OUTPUT); // AIN_2 digitaler Ausgang
  pinMode(5, OUTPUT); // BIN_1 digitaler Ausgang
  pinMode(7, OUTPUT); // BIN_2 digitaler Ausgang
  pinMode(6, OUTPUT); // PWM_B-Signal als Ausgang
  lcd.setCursor(0, 0);
  lcd.print("Taste druecken");
  delay(3000);
}

void loop() {
  lcd.clear();
  lcd.setCursor(0, 0); lcd.print("Hallo");
  
  if (btSerial.available()) {
    byte c = byte(btSerial.read()); // Cast last char to byte
    while (btSerial.available() > 0) c = byte(btSerial.read()); // This could (should) be done above, but idk
    char left = char(c & 0xF0); // interpret first four bits as signed char
    char right = char((c & 0x0F) << 4); // interpret last four bits as signed char
    writeMotor(left, right);
  }

  delay(50);
}

void writeMotor(char left, char right) {
  if (right == 0) {
    digitalWrite(2, LOW);
    digitalWrite(4, LOW);
  } else if (right > 0) {
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
    digitalWrite(3, byte(right) << 1);
  } else {
    digitalWrite(2, HIGH);
    digitalWrite(4, LOW);
    digitalWrite(3, byte(-(right + 1)) << 1);
  }

  if (left == 0) {
    digitalWrite(5, LOW);
    digitalWrite(7, LOW);
  } else if (left > 0) {
    digitalWrite(5, LOW);
    digitalWrite(7, HIGH);
    digitalWrite(6, byte(left) << 1);
  } else {
    digitalWrite(5, HIGH);
    digitalWrite(7, LOW);
    digitalWrite(6, byte(-(left + 1)) << 1);
  }
}