#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>
#include <KY040.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

KY040* g_rotaryEncoder1 = NULL;   // Pointer
KY040* g_rotaryEncoder2 = NULL;   // Pointer
KY040* g_rotaryEncoder3 = NULL;   // Pointer

// alte Werte merken
double oldKp = 0, oldKi = 0, oldKd = 0, oldIR = 0;
const double epsilon = 0.0001; // Empfindlichkeit für Vergleich

void setup() {
  // put your setup code here, to run once:
  lcd.init();
  lcd.backlight();
  g_rotaryEncoder1 = new KY040(25, 26);
  g_rotaryEncoder2 = new KY040(22, 23);
  g_rotaryEncoder3 = new KY040(28, 29);
}

void loop() {
  static int value1 = 0;
    static int value2 = 0;
    static int value3 = 0;

    switch (g_rotaryEncoder1->getRotation()) {
        case KY040::CLOCKWISE:
            value1++;
            break;
        case KY040::COUNTERCLOCKWISE:
            value1--;
            break; 
    }
    switch (g_rotaryEncoder2->getRotation()) {
        case KY040::CLOCKWISE:
            value2++;
            break;
        case KY040::COUNTERCLOCKWISE:
            value2--;
            break; 
    }
    switch (g_rotaryEncoder3->getRotation()) {
        case KY040::CLOCKWISE:
            value3++;
            break;
        case KY040::COUNTERCLOCKWISE:
            value3--;
            break; 
    }
  // put your main code here, to run repeatedly:
  double Kp = value1;
  double Ki = value2;
  double Kd = value3;
  double IR = 0.4;

  // Prüfen, ob sich etwas merklich geändert hat
  bool changed = false;
  if (fabs(Kp - oldKp) > epsilon) changed = true;
  if (fabs(Ki - oldKi) > epsilon) changed = true;
  if (fabs(Kd - oldKd) > epsilon) changed = true;
  if (fabs(IR - oldIR) > epsilon) changed = true;

  if (changed) {
    // Neue Werte anzeigen
    char bKp[10], bKi[10], bKd[10], bIR[10];
    dtostrf(Kp, 0, 2, bKp);
    dtostrf(Ki, 0, 2, bKi);
    dtostrf(Kd, 0, 2, bKd);
    dtostrf(IR, 0, 2, bIR);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Kp:");
    lcd.print(bKp);
    lcd.print(" ");
    lcd.print("Ki:");
    lcd.print(bKi);

    lcd.setCursor(0, 1);
    lcd.print("Kd:");
    lcd.print(bKd);
    lcd.print(" ");
    lcd.print("IR:");
    lcd.print(bIR);

    // Neue Werte als alte speichern
    oldKp = Kp;
    oldKi = Ki;
    oldKd = Kd;
    oldIR = IR;
  }

}
