//**************************************************************************
// Hochschule Hamm-Lippstadt *
//**************************************************************************
// Modul : LCD_I2C.cpp *
// *
// Datum : 26. Dezember 2025 *
// *
// Funktion : In diesem Block ist die Logik für das LCD Display des Kugel-
//              Balancierers implementiert. Je nach Modus werden die drei 
//              Regel-Parameter angezeigt oder eine Kalibriernachricht. 
//              Bei erfolgreichem Regeln der Kugel wird ein Code angezeigt.*
// *
// Implementierung : MATLAB R2024b *
// *
// Req. library : [https://github.com/johnrickman/LiquidCrystal_I2C LiquidCrystal I2C von Frank de Brabander]
// *
// Autor : Kilian Engelhardt, Daniel Block
// *
// Bemerkung : getestet
// *
// Letzte Änderung : 26. Dez 2025 
// *
//**************************************************************************

#include "C:\Users\Daniel\Documents\Uni\BSE\BSE_Semester_2\Praktikum\SVN\194_Kugel-Balancierer\Matlab_Simulink\LCD\LCD_I2C.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>

// Initialisierung des LCD: Adresse 0x27, 16 Spalten, 2 Zeilen
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Globale Variablen zum Speichern der Zustände des vorherigen Durchlaufs
double oldKp = 0, oldKi = 0, oldKd = 0, oldSollwert = 0;
const double epsilon = 0.0001; // Empfindlichkeit für Vergleich

// Initialisiert das LCD-Display beim Start des Systems
void setupFunctionLCD_I2C(){
    lcd.init();
    lcd.backlight();
}


// Kp double [1,1]
// Ki double [1,1]
// Kd double [1,1]
// Modus uint8 [1,1]
// Sollwert double [1,1]
// WIN logical [1,1]

void stepFunctionLCD_I2C(double Kp,int size_vector_a,double Ki,int size_vector_b,double Kd,int size_vector_c,uint8_T Modus,int size_vector_d,double Sollwert,int size_vector_e,boolean_T WIN,int size_vector_f){
    // Statische Flags, um einmalige Aktionen beim Zustandswechsel zu steuern
    static bool ersterDurchlauf = false;
    static bool kalibrierDurchlauf = false;
    static bool winDurchlauf = false;
    
    // Gewinnzustand
    if (WIN == true) {
        if (winDurchlauf == false) {    // Nur ausführen, wenn wir gerade erst gewonnen haben
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Gewonnen!");
            lcd.setCursor(0, 1);
            lcd.print("Code: 1234");
            winDurchlauf = true;

            // Andere Status-Flags zurücksetzen
            ersterDurchlauf = false;
            kalibrierDurchlauf = false;
        }
    }
    else {
        // Spielmodus (Modus == 1), Anzeige der PID-Parameter.
        if (fabs(Modus - 1) < 0.001) {
            bool changed;
            // Prüfen, ob dies der erste Wechsel in diesen Modus ist
            if (ersterDurchlauf == false) {
                changed = true;
                ersterDurchlauf = true;
            }
            else
                changed = false;
            // Prüfen, ob sich einer der PID-Werte geändert hat
            if (fabs(Kp - oldKp) > epsilon) changed = true;
            if (fabs(Ki - oldKi) > epsilon) changed = true;
            if (fabs(Kd - oldKd) > epsilon) changed = true;

            if (changed) {
                // Buffer für die Umwandlung von double zu String (dtostrf)
                char bKp[10], bKi[10], bKd[10];
                // Formatierung auf 2 Nachkommastellen
                dtostrf(Kp, 0, 2, bKp);
                dtostrf(Ki, 0, 2, bKi);
                dtostrf(Kd, 0, 2, bKd);

                // Anzeige der PID-Werte auf dem LCD
                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);

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

                kalibrierDurchlauf = false;
                winDurchlauf = false;
            }
        }
        // Kalibriermodus (Modus == 0)
        else if (fabs(Modus) < 0.001){
            bool kalibrierung = false;
            // Initialisierung der Anzeige beim Wechsel in den Kalibriermodus
            if (kalibrierDurchlauf == false) {
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Kalibriermodus");

                kalibrierDurchlauf = true;
                ersterDurchlauf = false;
                winDurchlauf = false;
            }
            // Prüfen, ob der Sollwert kalibriert wurde
            if (fabs(Sollwert - oldSollwert) > epsilon) kalibrierung = true;
            // Anzeige das Kalibrierung erfolgreich war
            if (kalibrierung) {
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Sollwert");
                lcd.setCursor(0, 1);
                lcd.print("kalibriert!");

                oldSollwert = Sollwert;
            }
        }
    }
}