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

HuskylensV2 huskylens;

// --- PIN-DEFINITIONEN ---
// TODO: Tragt hier die Pins ein, an die ihr eure LEDs auf dem Steckbrett angeschlossen habt.
int rotPin  = ; 
int blauPin = ; 

// --- VARIABLEN FÜR STATUS UND TIMING ---
// Wir nutzen eine sogenannte "State Machine" (Zustandsautomat), um uns zu merken, was gerade passiert.
enum LedStatus { KEINE, ROT, BLAU };
LedStatus aktuellerStatus = KEINE;

// Variablen für das nicht-blockierende Blinken
unsigned long letzterWechsel = 0;
bool ledAn = false;
const unsigned long blinkIntervall = 500; // Blink-Geschwindigkeit in Millisekunden

void setup() {
  // TODO: Konfiguriert die beiden LED-Pins als OUTPUT
  
  
  // Serielle Kommunikation und I2C-Bus starten
  Serial.begin(115200);
  Wire.begin(); 
  
  Serial.println("Verbinde mit HuskyLens 2...");
  
  // Warteschleife, bis die HuskyLens gefunden wurde
  while (!huskylens.begin(Wire)) {
    Serial.println("HuskyLens 2 nicht gefunden! Verkabelung pruefen...");
    delay(500);
  }
  
  // HuskyLens in den Farberkennungs-Modus schalten
  huskylens.switchAlgorithm(ALGORITHM_COLOR_RECOGNITION);
  Serial.println("HuskyLens 2 verbunden! Bereit.");
}

void loop() {
  // 1. Aktuelles Ergebnis der Farberkennung abholen
  // (Dieser Befehl ist vorgegeben, damit die Kamera neue Bilder verarbeitet)
  huskylens.getResult(ALGORITHM_COLOR_RECOGNITION);
  
  LedStatus neuerStatus = KEINE; // Standardannahme: Es wird gerade keine Farbe erkannt
  
  // 2. Auswertung der erkannten Farben
  if (huskylens.available(ALGORITHM_COLOR_RECOGNITION)) {
    
    // TODO: Prüfe, ob ID2 (Rot) erkannt wurde. Nutze dazu den Befehl aus dem Cheat-Sheet!
    // if (...) {
    //   neuerStatus = ROT;
    // }
    
    // TODO: Prüfe im Anschluss, ob ID1 (Blau) erkannt wurde.
    // else if (...) {
    //   neuerStatus = BLAU;
    // }
  }
  
  // 3. Statusänderung erkennen und im Monitor ausgeben
  // Wir führen diesen Block nur aus, wenn sich die gesehene Farbe geändert hat.
  if (neuerStatus != aktuellerStatus) {
    aktuellerStatus = neuerStatus; // Status aktualisieren
    
    // TODO: Wenn der Status ROT ist -> Schreibe "Rote LED aktiv" in den Monitor und schalte die blaue LED sicherheitshalber aus (LOW).
    
    // TODO: Wenn der Status BLAU ist -> Schreibe "Blaue LED aktiv" in den Monitor und schalte die rote LED aus.
    
    // TODO: Wenn der Status KEINE ist -> Schreibe "Nichts erkannt" und schalte BEIDE LEDs aus.
    
  }
  
  // 4. Nicht-blockierendes Blinken der aktiven LED (ohne delay!)
  // Wir blinken nur, wenn eine Farbe erkannt wurde.
  if (aktuellerStatus != KEINE) {
    
    // TODO: Überprüfe, ob die Zeit (blinkIntervall) abgelaufen ist. Nutze dafür millis() und letzterWechsel!
    // if (...) {
      
      // TODO: Aktualisiere 'letzterWechsel' auf die aktuelle Zeit.
      
      // TODO: Kehre den Zustand der Variable 'ledAn' um (aus true mach false, aus false mach true).
      
      // TODO: Schalte jetzt je nach 'aktuellerStatus' die richtige LED ein oder aus (nutze dafür den Wert von 'ledAn').
      
    // }
  }
  
  // Kleine Pause am Ende der Schleife, damit der I2C-Bus nicht mit Anfragen überflutet wird
  delay(50); 
}