/*
  HuskyLens 2 - Color Recognition Beispiel für ESP32
  ----------------------------------------------------
  Bibliothek: DFRobot_HuskylensV2
  https://github.com/DFRobot/DFRobot_HuskylensV2

  Verkabelung (I2C, HuskyLens 2 extern per Power Bank versorgt):
    HuskyLens SDA -> ESP32 SDA (Standard meist GPIO21, je nach Board)
    HuskyLens SCL -> ESP32 SCL (Standard meist GPIO22, je nach Board)
    HuskyLens GND -> ESP32 GND (gemeinsame Masse, zwingend nötig!)

  Wichtig:
    - Protokoll an der HuskyLens 2 selbst auf "I2C" oder "Auto Detect" stellen
      (System Settings -> Protocol Type)
    - Bibliothek vorher über den Arduino Library Manager installieren:
      Suche nach "DFRobot_HuskylensV2"
*/

#include <Wire.h>
#include "DFRobot_HuskylensV2.h"

// HuskyLens-Objekt anlegen
HuskylensV2 huskylens;

// Falls dein ESP32-Board andere I2C-Pins nutzt, hier anpassen:
#define I2C_SDA 21
#define I2C_SCL 22

void setup() {
  Serial.begin(115200);
  delay(500);

  // I2C mit den gewünschten Pins starten
  Wire.begin(I2C_SDA, I2C_SCL);

  Serial.println("Verbinde mit HuskyLens 2 ...");

  // Warten, bis die HuskyLens erfolgreich initialisiert werden konnte
  while (!huskylens.begin(Wire)) {
    Serial.println("HuskyLens 2 nicht gefunden! Verkabelung/Protokoll pruefen.");
    delay(500);
  }

  Serial.println("HuskyLens 2 verbunden!");

  // Auf den Color-Recognition-Algorithmus umschalten
  huskylens.switchAlgorithm(ALGORITHM_COLOR_RECOGNITION);

  Serial.println("Color Recognition aktiviert. Halte ein Objekt vor die Kamera.");
}

void loop() {
  // Neue Ergebnisse vom Sensor abrufen
  huskylens.getResult(ALGORITHM_COLOR_RECOGNITION);

  if (huskylens.available(ALGORITHM_COLOR_RECOGNITION)) {

    int anzahl = huskylens.getCachedResultNum(ALGORITHM_COLOR_RECOGNITION);
    Serial.print("Erkannte Farbobjekte: ");
    Serial.println(anzahl);

    // Das Objekt, das der Bildmitte am naechsten ist
    void* zentrum = huskylens.getCachedCenterResult(ALGORITHM_COLOR_RECOGNITION);

    if (zentrum != NULL) {
      int id      = RET_ITEM_NUM(zentrum, Result, ID);
      int xCenter = RET_ITEM_NUM(zentrum, Result, xCenter);
      int yCenter = RET_ITEM_NUM(zentrum, Result, yCenter);
      int breite  = RET_ITEM_NUM(zentrum, Result, width);
      int hoehe   = RET_ITEM_NUM(zentrum, Result, height);

      Serial.print("Farb-ID: ");
      Serial.print(id);
      Serial.print(" | Position: (");
      Serial.print(xCenter);
      Serial.print(", ");
      Serial.print(yCenter);
      Serial.print(") | Groesse: ");
      Serial.print(breite);
      Serial.print("x");
      Serial.println(hoehe);

      // Beispiel: einfache Reaktion je nach erkannter Farb-ID
      // (Die IDs werden vorher am Geraet selbst antrainiert/zugewiesen)
      switch (id) {
        case 1:
          Serial.println("Blaues Objekt");
          break;
        case 2:
          Serial.println("Rotes Objekt");
          break;
        default:
          Serial.println(">> Unbekannte/nicht zugeordnete Farb-ID");
          break;
      }
    }
  } else {
    Serial.println("Keine Farbe erkannt.");
  }

  delay(500);
}