#include <Arduino.h>

// Structure to represent a detection state
struct Detection {
  uint8_t zones;        // 8-bit unsigned integer for zones (up to 8 bits)
  bool stop;            // Stop flag
  uint8_t speed_limit;  // Speed limit in arbitrary units
};

// Structure to represent a zone
struct Zone {
  uint8_t id;           // Zone ID (0-7 for 8-bit field)
  bool stop;            // Zone stop flag
  uint8_t speed_limit;  // Zone speed limit
};

// Define LED pins for ESP32 (using GPIO pins)
const int ledPins[] = {13, 12, 14, 27}; // GPIO pins for first 4 bits of zones
const int stopLedPin = 26;              // GPIO for stop flag
const int speedLedPin = 25;             // GPIO for speed limit change indicator

void setup() {
  // Initialize Serial for debugging (ESP32 uses same Serial as Arduino)
  Serial.begin(115200); // Higher baud rate for ESP32

  // Initialize LED pins
  for (int i = 0; i < 4; i++) {
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], LOW); // Ensure LEDs start off
  }
  pinMode(stopLedPin, OUTPUT);
  digitalWrite(stopLedPin, LOW);
  pinMode(speedLedPin, OUTPUT);
  digitalWrite(speedLedPin, LOW);

  // Initialize detection state
  Detection detection = {0, false, 255}; // No zones, no stop, max speed limit

  // Example zones with uint8_t IDs (0-3 for demo, within 0-7 range)
  Zone zones[] = {
    {0, false, 60},  // Zone 0: bit 0, no stop, speed limit 60
    {1, true, 40},   // Zone 1: bit 1, stop, speed limit 40
    {2, false, 80},  // Zone 2: bit 2, no stop, speed limit 80
    {3, true, 30}    // Zone 3: bit 3, stop, speed limit 30
  };

  Serial.println("Initial Detection State:");
  printDetectionState(detection);

  // Process each zone
  for (int i = 0; i < 4; i++) {
    // Validate zone.id is within 0-7 for 8-bit field
    if (zones[i].id > 7) {
      Serial.print("Error: Zone ID ");
      Serial.print(zones[i].id);
      Serial.println(" exceeds 8-bit range (0-7)");
      continue;
    }

    Serial.print("Processing Zone ");
    Serial.println(zones[i].id);

    // Perform bit operation: Set zone bit using uint8_t id
    detection.zones |= (uint8_t)(1 << zones[i].id);

    // Update stop flag
    if (zones[i].stop) {
      detection.stop = true;
    }

    // Update speed limit if zone's speed limit is lower
    if (zones[i].speed_limit < detection.speed_limit) {
      detection.speed_limit = zones[i].speed_limit;
      digitalWrite(speedLedPin, HIGH); // Indicate speed limit change
    }

    // Visualize the current state
    updateLeds(detection);

    // Print current state
    Serial.print("After Zone ");
    Serial.print(zones[i].id);
    Serial.println(":");
    printDetectionState(detection);

    delay(2000); // Delay for visualization
    digitalWrite(speedLedPin, LOW); // Reset speed limit LED
  }
}

void loop() {
  // No continuous operation needed for this demo
}

// Function to print detection state via Serial
void printDetectionState(Detection detection) {
  Serial.print("Zones (binary): ");
  for (int i = 7; i >= 0; i--) {
    Serial.print((detection.zones >> i) & 1);
  }
  Serial.println();
  Serial.print("Stop: ");
  Serial.println(detection.stop ? "True" : "False");
  Serial.print("Speed Limit: ");
  Serial.println(detection.speed_limit);
}

// Function to update LEDs based on detection state
void updateLeds(Detection detection) {
  // Update LEDs for first 4 bits of zones
  for (int i = 0; i < 4; i++) {
    digitalWrite(ledPins[i], (detection.zones >> i) & 1 ? HIGH : LOW);
  }
  // Update stop LED
  digitalWrite(stopLedPin, detection.stop ? HIGH : LOW);
}