// Name: Joachim Hummel
// Erstellungsdatum: 22.01.2025
// Hardware: Vision AI V2 Kit


#include <Seeed_Arduino_SSCMA.h>
#include <WiFi.h>
#include <PubSubClient.h>

// Wi-Fi-Zugangsdaten
const char* ssid = "DEIN_WIFI_SSID";
const char* password = "DEIN_WIFI_PASSWORT";

// MQTT-Broker-Zugangsdaten
const char* mqtt_server = "DEINE_MQTT_BROKER_ADRESSE";
const char* mqtt_topic = "/vision_ai/data";

WiFiClient espClient;
PubSubClient client(espClient);
SSCMA AI;

void setupWiFi() {
    delay(10);
    Serial.println("Verbinde mit Wi-Fi...");
    WiFi.begin(ssid, password);

    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("\nWi-Fi verbunden.");
    Serial.println("IP-Adresse: " + WiFi.localIP().toString());
}

void reconnectMQTT() {
    while (!client.connected()) {
        Serial.print("Verbinde mit MQTT...");
        if (client.connect("Grove_Vision_AI")) {
            Serial.println("Verbunden.");
        } else {
            Serial.print("Fehler. Status: ");
            Serial.println(client.state());
            delay(5000);
        }
    }
}

void setup() {
    Serial.begin(115200); // Debug-Ausgabe
    AI.begin();

    // Wi-Fi und MQTT initialisieren
    setupWiFi();
    client.setServer(mqtt_server, 1883);
}

void loop() {
    // MQTT-Server verbunden halten
    if (!client.connected()) {
        reconnectMQTT();
    }
    client.loop();

    // Grove Vision AI verarbeiten
    if (!AI.invoke()) {
        Serial.println("Erkennung erfolgreich.");
        Serial.print("perf: prepocess=");
        Serial.print(AI.perf().prepocess);
        Serial.print(", inference=");
        Serial.print(AI.perf().inference);
        Serial.print(", postpocess=");
        Serial.println(AI.perf().postprocess);

        String mqttMessage = "{"; // MQTT-Nachricht erstellen
        mqttMessage += "\"perf\":{\"prepocess\":" + String(AI.perf().prepocess);
        mqttMessage += ",\"inference\":" + String(AI.perf().inference);
        mqttMessage += ",\"postprocess\":" + String(AI.perf().postprocess) + "},";

        mqttMessage += "\"boxes\": [";
        for (int i = 0; i < AI.boxes().size(); i++) {
            mqttMessage += "{\"target\":\"" + String(AI.boxes()[i].target) + "\",";
            mqttMessage += "\"score\":" + String(AI.boxes()[i].score) + ",";
            mqttMessage += "\"x\":" + String(AI.boxes()[i].x) + ",";
            mqttMessage += "\"y\":" + String(AI.boxes()[i].y) + ",";
            mqttMessage += "\"w\":" + String(AI.boxes()[i].w) + ",";
            mqttMessage += "\"h\":" + String(AI.boxes()[i].h) + "},";
        }
        if (AI.boxes().size() > 0) {
            mqttMessage.remove(mqttMessage.length() - 1); // Letztes Komma entfernen
        }
        mqttMessage += "]}";

        Serial.println("MQTT-Nachricht: " + mqttMessage);

        // MQTT-Nachricht senden
        if (client.publish(mqtt_topic, mqttMessage.c_str())) {
            Serial.println("Daten erfolgreich an MQTT gesendet!");
        } else {
            Serial.println("Fehler beim Senden der MQTT-Daten.");
        }
    }
}