Arduino 开发示例

// Arduino Multi-Sensor Monitoring Dashboard
// Complete sensor monitoring with LCD, SD card logging, and web interface
// Compatible with Arduino Uno/Mega and ESP32

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SD.h>
#include <SPI.h>
#include <DHT.h>
#include <BH1750.h>
#include <ArduinoJson.h>

// Pin configurations for Arduino Uno
#define DHT_PIN 2
#define DHT_TYPE DHT22
#define LIGHT_SENSOR_PIN A0
#define SOUND_SENSOR_PIN A1
#define PIR_PIN 3
#define BUZZER_PIN 4
#define LED_PIN 13
#define SD_CS_PIN 10

// I2C addresses
#define LCD_ADDRESS 0x27
#define BH1750_ADDRESS 0x23

// Timing constants
#define SENSOR_UPDATE_INTERVAL 2000    // 2 seconds
#define LOGGING_INTERVAL 10000          // 10 seconds
#define DISPLAY_UPDATE_INTERVAL 1000    // 1 second
#define WEB_UPDATE_INTERVAL 5000        // 5 seconds

// Sensor thresholds
#define TEMP_HIGH_THRESHOLD 30.0
#define TEMP_LOW_THRESHOLD 10.0
#define HUMIDITY_HIGH_THRESHOLD 70.0
#define LIGHT_HIGH_THRESHOLD 800
#define SOUND_HIGH_THRESHOLD 600

// Global objects
LiquidCrystal_I2C lcd(0x27, 20, 4);
DHT dht(DHT_PIN, DHT_TYPE);
BH1750 lightMeter;
File dataFile;

// Sensor data structure
struct SensorData {
    float temperature;
    float humidity;
    float lightLevel;
    int soundLevel;
    bool motionDetected;
    unsigned long timestamp;
    String dateTime;
    int freeMemory;
};

// System state
SensorData currentData;
SensorData historicalData[100];
int dataIndex = 0;
bool sdCardAvailable = false;
bool alertActive = false;
unsigned long lastSensorUpdate = 0;
unsigned long lastLoggingUpdate = 0;
unsigned long lastDisplayUpdate = 0;
unsigned long lastWebUpdate = 0;

// Alert counters
int tempAlertCount = 0;
int humidityAlertCount = 0;
int lightAlertCount = 0;
int soundAlertCount = 0;

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

    Serial.println("\n=== Arduino Multi-Sensor Dashboard ===");
    Serial.println("Initializing system...");

    // Initialize pins
    pinMode(LED_PIN, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);
    pinMode(PIR_PIN, INPUT);
    digitalWrite(LED_PIN, LOW);
    digitalWrite(BUZZER_PIN, LOW);

    // Initialize LCD
    initializeLCD();

    // Initialize sensors
    initializeSensors();

    // Initialize SD card
    initializeSDCard();

    // Initialize motion detection interrupt
    attachInterrupt(digitalPinToInterrupt(PIR_PIN), motionDetected, RISING);

    // Display welcome message
    displayWelcomeMessage();

    // Initialize web server (for ESP32)
    #ifdef ESP32
    initializeWebServer();
    #endif

    Serial.println("System initialized successfully!");
}

void loop() {
    unsigned long currentTime = millis();

    // Update sensor data
    if (currentTime - lastSensorUpdate >= SENSOR_UPDATE_INTERVAL) {
        updateSensorData();
        lastSensorUpdate = currentTime;

        // Store historical data
        storeHistoricalData();

        // Check for alerts
        checkAlerts();
    }

    // Update LCD display
    if (currentTime - lastDisplayUpdate >= DISPLAY_UPDATE_INTERVAL) {
        updateDisplay();
        lastDisplayUpdate = currentTime;
    }

    // Log data to SD card
    if (sdCardAvailable && (currentTime - lastLoggingUpdate >= LOGGING_INTERVAL)) {
        logDataToSDCard();
        lastLoggingUpdate = currentTime;
    }

    // Update web interface (ESP32 only)
    #ifdef ESP32
    if (currentTime - lastWebUpdate >= WEB_UPDATE_INTERVAL) {
        updateWebInterface();
        lastWebUpdate = currentTime;
    }
    #endif

    // Handle alerts
    handleAlerts();

    delay(100);
}

void initializeLCD() {
    Serial.println("Initializing LCD...");

    lcd.init(20, 4);
    lcd.backlight();

    // Test LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Sensor Dashboard");
    lcd.setCursor(0, 1);
    lcd.print("Initializing...");

    delay(2000);
    Serial.println("LCD initialized");
}

void initializeSensors() {
    Serial.println("Initializing sensors...");

    // Initialize DHT22 temperature/humidity sensor
    dht.begin();

    // Initialize BH1750 light sensor
    if (lightMeter.begin(BH1750_ADDRESS, CONTINUOUS_HIGH_RES_MODE)) {
        Serial.println("BH1750 light sensor initialized");
    } else {
        Serial.println("Error initializing BH1750 light sensor");
    }

    // Test sensors
    float testTemp = dht.readTemperature();
    float testHum = dht.readHumidity();

    if (isnan(testTemp) || isnan(testHum)) {
        Serial.println("Warning: DHT22 sensor not responding");
    }

    Serial.println("Sensors initialized");
}

void initializeSDCard() {
    Serial.println("Initializing SD card...");

    if (SD.begin(SD_CS_PIN)) {
        sdCardAvailable = true;
        Serial.println("SD card initialized successfully");

        // Test file creation
        if (SD.exists("sensor_data.csv")) {
            Serial.println("Existing data file found");
        } else {
            createDataFile();
        }
    } else {
        sdCardAvailable = false;
        Serial.println("Error initializing SD card");
    }
}

void createDataFile() {
    dataFile = SD.open("sensor_data.csv", FILE_WRITE);
    if (dataFile) {
        // Write CSV header
        dataFile.println("Timestamp,Temperature,Humidity,Light,Sound,Motion,FreeMemory");
        dataFile.close();
        Serial.println("Created new data file");
    } else {
        Serial.println("Error creating data file");
    }
}

void displayWelcomeMessage() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Arduino Sensor");
    lcd.setCursor(0, 1);
    lcd.print("Monitoring System");
    lcd.setCursor(0, 2);
    lcd.print("Ready!");
    lcd.setCursor(0, 3);
    lcd.print("Initializing...");

    delay(3000);
    lcd.clear();
}

void updateSensorData() {
    // Read temperature and humidity from DHT22
    currentData.temperature = dht.readTemperature();
    currentData.humidity = dht.readHumidity();

    // Handle DHT22 reading errors
    if (isnan(currentData.temperature) || isnan(currentData.humidity)) {
        Serial.println("Error reading DHT22 sensor");
        // Use last valid values or defaults
        if (dataIndex > 0) {
            currentData.temperature = historicalData[dataIndex - 1].temperature;
            currentData.humidity = historicalData[dataIndex - 1].humidity;
        } else {
            currentData.temperature = 20.0;
            currentData.humidity = 50.0;
        }
    }

    // Read light level from BH1750
    currentData.lightLevel = lightMeter.readLightLevel();
    if (currentData.lightLevel < 0) {
        currentData.lightLevel = analogRead(LIGHT_SENSOR_PIN);
        currentData.lightLevel = map(currentData.lightLevel, 0, 1023, 0, 1000);
    }

    // Read sound level from analog sensor
    currentData.soundLevel = analogRead(SOUND_SENSOR_PIN);

    // Motion is handled by interrupt
    // currentData.motionDetected is set in motionDetected() function

    // Get current timestamp
    currentData.timestamp = millis();
    currentData.dateTime = getCurrentDateTime();

    // Get free memory
    currentData.freeMemory = getFreeMemory();

    // Serial output for debugging
    Serial.printf("Temp: %.1f°C, Humidity: %.1f%%, Light: %.1f lux, Sound: %d, Motion: %s\n",
                 currentData.temperature, currentData.humidity, currentData.lightLevel,
                 currentData.soundLevel, currentData.motionDetected ? "Yes" : "No");
}

void storeHistoricalData() {
    historicalData[dataIndex] = currentData;
    dataIndex = (dataIndex + 1) % 100;  // Circular buffer of 100 entries
}

String getCurrentDateTime() {
    char dateTimeStr[20];
    unsigned long seconds = currentData.timestamp / 1000;
    unsigned long minutes = seconds / 60;
    unsigned long hours = minutes / 60;
    unsigned long days = hours / 24;

    sprintf(dateTimeStr, "%02ld:%02ld:%02ld:%02ld",
            days, hours % 24, minutes % 60, seconds % 60);

    return String(dateTimeStr);
}

int getFreeMemory() {
    #ifdef ESP32
        return ESP.getFreeHeap();
    #else
        extern int __heap_start, *__brkval;
        int v;
        return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
    #endif
}

void updateDisplay() {
    lcd.clear();

    // Line 1: Temperature and Humidity
    lcd.setCursor(0, 0);
    lcd.printf("T:%.1f°C H:%.0f%%", currentData.temperature, currentData.humidity);

    // Line 2: Light and Sound
    lcd.setCursor(0, 1);
    lcd.printf("L:%.0f S:%4d", currentData.lightLevel, currentData.soundLevel);

    // Line 3: Motion and Memory
    lcd.setCursor(0, 2);
    lcd.printf("M:%s Mem:%dB", currentData.motionDetected ? "Yes" : "No ", currentData.freeMemory / 100);

    // Line 4: Status and alerts
    lcd.setCursor(0, 3);
    if (alertActive) {
        lcd.printf("ALERT! %s", getActiveAlert().c_str());
    } else {
        lcd.printf("Status: OK");
    }
}

void checkAlerts() {
    alertActive = false;

    // Temperature alerts
    if (currentData.temperature > TEMP_HIGH_THRESHOLD) {
        triggerAlert("TEMP_HIGH", "Temperature too high");
        tempAlertCount++;
    } else if (currentData.temperature < TEMP_LOW_THRESHOLD) {
        triggerAlert("TEMP_LOW", "Temperature too low");
        tempAlertCount++;
    } else {
        tempAlertCount = 0;
    }

    // Humidity alerts
    if (currentData.humidity > HUMIDITY_HIGH_THRESHOLD) {
        triggerAlert("HUMID_HIGH", "Humidity too high");
        humidityAlertCount++;
    } else {
        humidityAlertCount = 0;
    }

    // Light alerts
    if (currentData.lightLevel > LIGHT_HIGH_THRESHOLD) {
        triggerAlert("LIGHT_HIGH", "Light level too high");
        lightAlertCount++;
    } else {
        lightAlertCount = 0;
    }

    // Sound alerts
    if (currentData.soundLevel > SOUND_HIGH_THRESHOLD) {
        triggerAlert("SOUND_HIGH", "Noise level too high");
        soundAlertCount++;
    } else {
        soundAlertCount = 0;
    }

    // Motion alert (only trigger if motion is detected)
    if (currentData.motionDetected) {
        triggerAlert("MOTION", "Motion detected");
    }
}

void triggerAlert(String alertType, String message) {
    alertActive = true;
    Serial.printf("ALERT [%s]: %s\n", alertType.c_str(), message.c_str());
}

String getActiveAlert() {
    if (tempAlertCount > 0) return "TEMP";
    if (humidityAlertCount > 0) return "HUMID";
    if (lightAlertCount > 0) return "LIGHT";
    if (soundAlertCount > 0) return "SOUND";
    if (currentData.motionDetected) return "MOTION";
    return "";
}

void handleAlerts() {
    static unsigned long lastAlertTime = 0;
    static bool lastAlertState = false;
    static bool buzzerState = false;

    if (alertActive) {
        // Blink LED
        digitalWrite(LED_PIN, (millis() / 500) % 2);

        // Sound buzzer (if enabled and not too frequent)
        if (millis() - lastAlertTime > 1000) {
            buzzerState = !buzzerState;
            digitalWrite(BUZZER_PIN, buzzerState ? HIGH : LOW);
            lastAlertTime = millis();
        }
    } else {
        // Turn off alert indicators
        if (lastAlertState) {
            digitalWrite(LED_PIN, LOW);
            digitalWrite(BUZZER_PIN, LOW);
            lastAlertState = false;
        }
    }
}

void logDataToSDCard() {
    if (!sdCardAvailable) return;

    dataFile = SD.open("sensor_data.csv", FILE_WRITE);
    if (dataFile) {
        // Append new data
        dataFile.printf("%lu,%.2f,%.2f,%.2f,%d,%s,%d\n",
                       currentData.timestamp,
                       currentData.temperature,
                       currentData.humidity,
                       currentData.lightLevel,
                       currentData.soundLevel,
                       currentData.motionDetected ? "1" : "0",
                       currentData.freeMemory);
        dataFile.close();

        Serial.println("Data logged to SD card");
    } else {
        Serial.println("Error writing to SD card");
    }
}

void motionDetected() {
    currentData.motionDetected = true;
    Serial.println("Motion detected!");
}

#ifdef ESP32
// ESP32 Web Server Implementation
#include <WiFi.h>
#include <WebServer.h>
#include <WiFiClient.h>

const char* ssid = "YOUR_WIFI_SSID";
const char* password = "YOUR_WIFI_PASSWORD";

WebServer server(80);

void initializeWebServer() {
    // WiFi setup for ESP32
    WiFi.begin(ssid, password);

    while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.println("Connecting to WiFi...");
    }

    Serial.println("WiFi connected");
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());

    // Web server routes
    server.on("/", handleRoot);
    server.on("/data.json", handleData);
    server.on("/history.json", handleHistory);
    server.on("/stats", handleStats);

    server.begin();
    Serial.println("Web server started");
}

void handleRoot() {
    String html = generateWebPage();
    server.send(200, "text/html", html);
}

void handleData() {
    String json = generateCurrentDataJSON();
    server.send(200, "application/json", json);
}

void handleHistory() {
    String json = generateHistoryJSON();
    server.send(200, "application/json", json);
}

void handleStats() {
    String json = generateStatsJSON();
    server.send(200, "application/json", json);
}

void updateWebInterface() {
    // Web interface updates are handled by HTTP requests
}

String generateWebPage() {
    String html = R"(
<!DOCTYPE html>
<html>
<head>
    <title>Arduino Sensor Dashboard</title>
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <style>
        body { font-family: Arial, sans-serif; margin: 20px; }
        .dashboard { display: grid; grid-template-columns: repeat(auto-fit, minmax(300px, 1fr)); gap: 20px; }
        .card { background: #f9f9f9; padding: 20px; border-radius: 8px; box-shadow: 0 2px 4px rgba(0,0,0,0.1); }
        .card h3 { margin-top: 0; color: #333; }
        .metric { font-size: 2em; font-weight: bold; color: #007bff; }
        .unit { font-size: 0.6em; color: #666; }
        .alert { background: #f8d7da; border: 1px solid #f5c6cb; color: #721c24; }
        .ok { background: #d4edda; border: 1px solid #c3e6cb; color: #155724; }
        button { background: #007bff; color: white; border: none; padding: 10px 20px; border-radius: 4px; cursor: pointer; }
        button:hover { background: #0056b3; }
    </style>
</head>
<body>
    <h1>🌡️ Arduino Sensor Dashboard</h1>

    <div class="dashboard">
        <div class="card">
            <h3>Temperature</h3>
            <div class="metric"><span id="temperature">--</span><span class="unit">°C</span></div>
        </div>

        <div class="card">
            <h3>Humidity</h3>
            <div class="metric"><span id="humidity">--</span><span class="unit">%</span></div>
        </div>

        <div class="card">
            <h3>Light Level</h3>
            <div class="metric"><span id="light">--</span><span class="unit"> lux</span></div>
        </div>

        <div class="card">
            <h3>Sound Level</h3>
            <div class="metric"><span id="sound">--</span><span class="unit"> dB</span></div>
        </div>

        <div class="card">
            <h3>Status</h3>
            <div id="status" class="ok">System OK</div>
        </div>

        <div class="card">
            <h3>Controls</h3>
            <button onclick="location.reload()">🔄 Refresh</button>
            <button onclick="downloadData()">📊 Download Data</button>
        </div>
    </div>

    <script>
        async function updateData() {
            try {
                const response = await fetch('/data.json');
                const data = await response.json();

                document.getElementById('temperature').textContent = data.temperature.toFixed(1);
                document.getElementById('humidity').textContent = data.humidity.toFixed(1);
                document.getElementById('light').textContent = data.lightLevel.toFixed(0);
                document.getElementById('sound').textContent = data.soundLevel;

                const statusEl = document.getElementById('status');
                if (data.alertActive) {
                    statusEl.textContent = '⚠️ ' + data.activeAlert;
                    statusEl.className = 'alert';
                } else {
                    statusEl.textContent = '✅ System OK';
                    statusEl.className = 'ok';
                }
            } catch (error) {
                console.error('Error updating data:', error);
            }
        }

        function downloadData() {
            window.open('/history.json', '_blank');
        }

        // Update data every 2 seconds
        setInterval(updateData, 2000);
        updateData(); // Initial update
    </script>
</body>
</html>)";
    return html;
}

String generateCurrentDataJSON() {
    DynamicJsonDocument doc(512);
    doc["temperature"] = currentData.temperature;
    doc["humidity"] = currentData.humidity;
    doc["lightLevel"] = currentData.lightLevel;
    doc["soundLevel"] = currentData.soundLevel;
    doc["motionDetected"] = currentData.motionDetected;
    doc["timestamp"] = currentData.timestamp;
    doc["alertActive"] = alertActive;
    doc["activeAlert"] = getActiveAlert();
    doc["freeMemory"] = currentData.freeMemory;

    String jsonString;
    serializeJson(doc, jsonString);
    return jsonString;
}

String generateHistoryJSON() {
    DynamicJsonDocument doc(4096);
    JsonArray array = doc.to<JsonArray>();

    for (int i = 0; i < 20; i++) {
        int index = (dataIndex - 1 - i + 100) % 100;
        JsonObject obj = array.createNestedObject();
        obj["temperature"] = historicalData[index].temperature;
        obj["humidity"] = historicalData[index].humidity;
        obj["lightLevel"] = historicalData[index].lightLevel;
        obj["timestamp"] = historicalData[index].timestamp;
    }

    String jsonString;
    serializeJson(doc, jsonString);
    return jsonString;
}

String generateStatsJSON() {
    DynamicJsonDocument doc(512);
    doc["totalReadings"] = dataIndex;
    doc["tempAlerts"] = tempAlertCount;
    doc["humidityAlerts"] = humidityAlertCount;
    doc["lightAlerts"] = lightAlertCount;
    doc["soundAlerts"] = soundAlertCount;
    doc["uptime"] = millis();
    doc["freeMemory"] = currentData.freeMemory;
    doc["sdCardAvailable"] = sdCardAvailable;

    String jsonString;
    serializeJson(doc, jsonString);
    return jsonString;
}

#else
// Arduino Uno/Leo compatibility functions
void initializeWebServer() {
    // Not implemented for Arduino Uno (no WiFi)
    Serial.println("Web server not available on this platform");
}

void updateWebInterface() {
    // Not implemented for Arduino Uno
}

void handleRoot() {}
void handleData() {}
void handleHistory() {}
void handleStats() {}
String generateWebPage() { return ""; }
String generateCurrentDataJSON() { return ""; }
String generateHistoryJSON() { return ""; }
String generateStatsJSON() { return ""; }
#endif

// Utility functions
void printSystemInfo() {
    Serial.println("\n=== System Information ===");
    Serial.printf("Uptime: %lu ms\n", millis());
    Serial.printf("Free Memory: %d bytes\n", getFreeMemory());
    Serial.printf("SD Card: %s\n", sdCardAvailable ? "Available" : "Not Available");
    Serial.printf("Total Readings: %d\n", dataIndex);
    Serial.printf("Temperature Alerts: %d\n", tempAlertCount);
    Serial.printf("Humidity Alerts: %d\n", humidityAlertCount);
    Serial.printf("Light Alerts: %d\n", lightAlertCount);
    Serial.printf("Sound Alerts: %d\n", soundAlertCount);
}

void formatSerialOutput() {
    // Clean up serial output formatting
    Serial.println();
    Serial.println("========================================");
    Serial.println();
}

// Emergency functions
void emergencyShutdown() {
    Serial.println("Emergency shutdown initiated!");

    // Save current data before shutdown
    logDataToSDCard();

    // Turn off all outputs
    digitalWrite(LED_PIN, LOW);
    digitalWrite(BUZZER_PIN, LOW);

    // Display shutdown message
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("SYSTEM SHUTDOWN");
    lcd.setCursor(0, 1);
    lcd.print("Data Saved");

    delay(5000);

    // Enter low power mode (if supported)
    #ifdef ESP32
    esp_deep_sleep_start();
    #endif
}

// Arduino Robot Controller
// Complete autonomous robot with motor control, navigation, and sensors
// Compatible with Arduino Mega and motor driver L298N or similar

#include <Arduino.h>
#include <Servo.h>
#include <NewPing.h>

// Motor driver pins (L298N)
#define ENA 5
#define IN1 6
#define IN2 7
#define ENB 10
#define IN3 8
#define IN4 9

// Ultrasonic sensor pins
#define TRIG_PIN 12
#define ECHO_PIN 13

// Line sensor pins (analog)
#define LINE_LEFT_PIN A0
#define LINE_CENTER_PIN A1
#define LINE_RIGHT_PIN A2

// Servo pin for ultrasonic sensor
#define SERVO_PIN 11

// LED and buzzer pins
#define STATUS_LED 2
#define BUZZER_PIN 3

// Navigation constants
#define MAX_DISTANCE 200      // Maximum detection distance in cm
#define SAFE_DISTANCE 30       // Safe distance from obstacles
#define WARNING_DISTANCE 60    // Warning distance for obstacles
#define BASE_SPEED 150         // Base motor speed
#define TURN_SPEED 100         // Speed during turns
#define LINE_THRESHOLD 512     // Threshold for line detection

// Robot states
enum RobotState {
    IDLE,
    MOVING_FORWARD,
    TURNING_LEFT,
    TURNING_RIGHT,
    OBSTACLE_AVOIDANCE,
    LINE_FOLLOWING,
    STOPPED,
    EMERGENCY
};

// Global variables
Servo ultrasonicServo;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
RobotState currentState = IDLE;
RobotState previousState = IDLE;

// Navigation variables
int currentSpeed = BASE_SPEED;
int turnAngle = 90;  // Center position for ultrasonic servo
unsigned long lastObstacleCheck = 0;
unsigned long lastLineCheck = 0;
bool lineDetected = false;
int linePosition = 0;  // -1: left, 0: center, 1: right

// Motor control variables
bool motorsEnabled = true;
unsigned long lastMotorUpdate = 0;

// Statistics
unsigned long totalDistance = 0;
unsigned int obstaclesAvoided = 0;
unsigned int turnsMade = 0;
unsigned long operationTime = 0;

// PID controller for line following
struct PIDController {
    float kp, ki, kd;
    float integral, previous_error;

    PIDController(float p = 1.0, float i = 0.0, float d = 0.0) {
        kp = p;
        ki = i;
        kd = d;
        integral = 0;
        previous_error = 0;
    }

    float calculate(float error) {
        integral += error;
        float derivative = error - previous_error;
        previous_error = error;
        return kp * error + ki * integral + kd * derivative;
    }

    void reset() {
        integral = 0;
        previous_error = 0;
    }
};

PIDController linePID(2.0, 0.1, 0.5);

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

    Serial.println("=== Arduino Robot Controller ===");
    Serial.println("Initializing robot systems...");

    // Initialize motor pins
    initializeMotors();

    // Initialize sensors
    initializeSensors();

    // Initialize servos
    initializeServos();

    // Initialize control pins
    pinMode(STATUS_LED, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);

    // Perform system checks
    performSystemCheck();

    // Ready signal
    signalReady();

    operationTime = millis();
    Serial.println("Robot ready! Starting navigation...");

    // Start in line following mode
    currentState = LINE_FOLLOWING;
}

void loop() {
    // Update sensors
    updateSensors();

    // State machine
    switch (currentState) {
        case IDLE:
            handleIdleState();
            break;

        case MOVING_FORWARD:
            handleForwardMovement();
            break;

        case TURNING_LEFT:
            handleLeftTurn();
            break;

        case TURNING_RIGHT:
            handleRightTurn();
            break;

        case OBSTACLE_AVOIDANCE:
            handleObstacleAvoidance();
            break;

        case LINE_FOLLOWING:
            handleLineFollowing();
            break;

        case STOPPED:
            handleStoppedState();
            break;

        case EMERGENCY:
            handleEmergencyState();
            break;
    }

    // Update status
    updateRobotStatus();

    // Check for manual overrides or commands
    checkManualCommands();

    delay(50);  // Main loop delay
}

void initializeMotors() {
    Serial.println("Initializing motors...");

    // Set motor pins as outputs
    pinMode(ENA, OUTPUT);
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(ENB, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);

    // Initialize motor states
    stopMotors();

    // Enable motors
    digitalWrite(ENA, HIGH);
    digitalWrite(ENB, HIGH);

    Serial.println("Motors initialized");
}

void initializeSensors() {
    Serial.println("Initializing sensors...");

    // Initialize ultrasonic servo
    ultrasonicServo.attach(SERVO_PIN);
    ultrasonicServo.write(turnAngle);

    // Initialize line sensor pins
    pinMode(LINE_LEFT_PIN, INPUT);
    pinMode(LINE_CENTER_PIN, INPUT);
    pinMode(LINE_RIGHT_PIN, INPUT);

    Serial.println("Sensors initialized");
}

void initializeServos() {
    Serial.println("Initializing servos...");

    // Center the ultrasonic servo
    ultrasonicServo.write(90);
    delay(500);

    Serial.println("Servos initialized");
}

void performSystemCheck() {
    Serial.println("Performing system check...");

    // Test motors
    Serial.println("Testing motors...");
    testMotors();

    // Test sensors
    Serial.println("Testing sensors...");
    testSensors();

    // Test servos
    Serial.println("Testing servos...");
    testServos();

    Serial.println("System check completed");
}

void testMotors() {
    // Test forward movement
    moveForward(BASE_SPEED);
    delay(1000);
    stopMotors();

    // Test turning
    turnLeft(90);
    delay(1000);
    turnRight(90);
    delay(1000);
    stopMotors();

    Serial.println("Motor test completed");
}

void testSensors() {
    // Test ultrasonic sensor
    int distance = readUltrasonic();
    Serial.printf("Ultrasonic reading: %d cm\n", distance);

    // Test line sensors
    int leftLine = analogRead(LINE_LEFT_PIN);
    int centerLine = analogRead(LINE_CENTER_PIN);
    int rightLine = analogRead(LINE_RIGHT_PIN);

    Serial.printf("Line sensors - Left: %d, Center: %d, Right: %d\n",
                 leftLine, centerLine, rightLine);
}

void testServos() {
    // Test servo movement
    for (int angle = 60; angle <= 120; angle += 10) {
        ultrasonicServo.write(angle);
        delay(100);
    }
    ultrasonicServo.write(90);

    Serial.println("Servo test completed");
}

void signalReady() {
    // Flash LED and beep buzzer to signal readiness
    for (int i = 0; i < 3; i++) {
        digitalWrite(STATUS_LED, HIGH);
        tone(BUZZER_PIN, 1000, 100);
        delay(200);
        digitalWrite(STATUS_LED, LOW);
        delay(200);
    }
}

// Motor control functions
void moveForward(int speed) {
    if (!motorsEnabled) return;

    // Left motor forward
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, speed);

    // Right motor forward
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, speed);

    Serial.printf("Moving forward at speed %d\n", speed);
}

void moveBackward(int speed) {
    if (!motorsEnabled) return;

    // Left motor backward
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, speed);

    // Right motor backward
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, speed);

    Serial.printf("Moving backward at speed %d\n", speed);
}

void turnLeft(int degrees) {
    if (!motorsEnabled) return;

    // Calculate turn duration based on degrees
    int turnDuration = map(degrees, 0, 360, 0, 2000);

    // Turn motors in opposite directions
    // Left motor backward
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, TURN_SPEED);

    // Right motor forward
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, TURN_SPEED);

    delay(turnDuration);
    stopMotors();

    turnsMade++;
    Serial.printf("Turned left %d degrees\n", degrees);
}

void turnRight(int degrees) {
    if (!motorsEnabled) return;

    // Calculate turn duration based on degrees
    int turnDuration = map(degrees, 0, 360, 0, 2000);

    // Turn motors in opposite directions
    // Left motor forward
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, TURN_SPEED);

    // Right motor backward
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, TURN_SPEED);

    delay(turnDuration);
    stopMotors();

    turnsMade++;
    Serial.printf("Turned right %d degrees\n", degrees);
}

void stopMotors() {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    analogWrite(ENA, 0);
    analogWrite(ENB, 0);
}

void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    if (!motorsEnabled) return;

    // Left motor
    if (leftSpeed >= 0) {
        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);
        analogWrite(ENA, leftSpeed);
    } else {
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, HIGH);
        analogWrite(ENA, -leftSpeed);
    }

    // Right motor
    if (rightSpeed >= 0) {
        digitalWrite(IN3, HIGH);
        digitalWrite(IN4, LOW);
        analogWrite(ENB, rightSpeed);
    } else {
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, HIGH);
        analogWrite(ENB, -rightSpeed);
    }
}

// Sensor functions
int readUltrasonic() {
    int distance = sonar.ping_cm();
    return distance;
}

void scanWithUltrasonic() {
    // Scan environment by moving servo
    int distances[9];

    for (int i = 0; i < 9; i++) {
        int angle = 60 + i * 15;  // 60° to 180°
        ultrasonicServo.write(angle);
        delay(50);  // Wait for servo to reach position
        distances[i] = readUltrasonic();
        delay(50);  // Wait for ultrasonic reading
    }

    // Return servo to center position
    ultrasonicServo.write(90);

    // Find the direction with the most open path
    int maxDistance = 0;
    int bestDirection = 90;

    for (int i = 0; i < 9; i++) {
        if (distances[i] > maxDistance) {
            maxDistance = distances[i];
            bestDirection = 60 + i * 15;
        }
    }

    Serial.printf("Best direction: %d°, Distance: %d cm\n", bestDirection, maxDistance);
    turnAngle = bestDirection;
}

void updateLineSensors() {
    int leftValue = analogRead(LINE_LEFT_PIN);
    int centerValue = analogRead(LINE_CENTER_PIN);
    int rightValue = analogRead(LINE_RIGHT_PIN);

    // Determine line position
    bool leftDetected = leftValue > LINE_THRESHOLD;
    bool centerDetected = centerValue > LINE_THRESHOLD;
    bool rightDetected = rightValue > LINE_THRESHOLD;

    if (leftDetected && rightDetected && !centerDetected) {
        linePosition = 0;  // Centered on wide line
    } else if (leftDetected) {
        linePosition = -1;  // Line is to the left
    } else if (rightDetected) {
        linePosition = 1;   // Line is to the right
    } else if (centerDetected) {
        linePosition = 0;  // Centered
    } else {
        linePosition = 2;  // No line detected
    }

    lineDetected = (linePosition < 2);

    // Debug output
    static unsigned long lastDebug = 0;
    if (millis() - lastDebug > 1000) {
        Serial.printf("Line sensors - L:%d C:%d R:%d Position:%d\n",
                     leftValue, centerValue, rightValue, linePosition);
        lastDebug = millis();
    }
}

void updateSensors() {
    // Update ultrasonic reading
    if (millis() - lastObstacleCheck > 100) {  // Check every 100ms
        readUltrasonic();
        lastObstacleCheck = millis();
    }

    // Update line sensors
    if (millis() - lastLineCheck > 50) {  // Check every 50ms
        updateLineSensors();
        lastLineCheck = millis();
    }
}

// State handling functions
void handleIdleState() {
    stopMotors();
    digitalWrite(STATUS_LED, LOW);

    // Wait for 3 seconds, then start line following
    static unsigned long idleStartTime = 0;
    if (idleStartTime == 0) {
        idleStartTime = millis();
    }

    if (millis() - idleStartTime > 3000) {
        currentState = LINE_FOLLOWING;
        idleStartTime = 0;
    }
}

void handleForwardMovement() {
    moveForward(currentSpeed);

    // Check for obstacles
    int distance = readUltrasonic();
    if (distance < SAFE_DISTANCE) {
        currentState = OBSTACLE_AVOIDANCE;
    }

    // Check for line
    updateLineSensors();
    if (lineDetected && linePosition != 2) {
        currentState = LINE_FOLLOWING;
    }
}

void handleLeftTurn() {
    // Turn left logic is handled in turnLeft() function
    // After turn is complete, return to previous state
    if (previousState == OBSTACLE_AVOIDANCE) {
        currentState = MOVING_FORWARD;
    } else {
        currentState = LINE_FOLLOWING;
    }
}

void handleRightTurn() {
    // Turn right logic is handled in turnRight() function
    // After turn is complete, return to previous state
    if (previousState == OBSTACLE_AVOIDANCE) {
        currentState = MOVING_FORWARD;
    } else {
        currentState = LINE_FOLLOWING;
    }
}

void handleObstacleAvoidance() {
    static unsigned long avoidanceStartTime = 0;
    static bool scanning = false;

    if (!scanning) {
        Serial.println("Scanning for clear path...");
        scanWithUltrasonic();
        scanning = true;
        avoidanceStartTime = millis();
    }

    // Wait for scan to complete
    if (millis() - avoidanceStartTime > 1500) {
        scanning = false;

        // Turn in the best direction
        if (turnAngle < 90) {
            turnLeft(90 - turnAngle);
        } else if (turnAngle > 90) {
            turnRight(turnAngle - 90);
        } else {
            // Best direction is straight, but we have an obstacle
            // Turn around
            turnRight(180);
        }

        obstaclesAvoided++;
        previousState = OBSTACLE_AVOIDANCE;
        currentState = MOVING_FORWARD;
    }
}

void handleLineFollowing() {
    if (!lineDetected || linePosition == 2) {
        // No line detected, move forward and search
        moveForward(BASE_SPEED);

        // Scan for line
        static unsigned long lastLineScan = 0;
        if (millis() - lastLineScan > 1000) {  // Scan every second
            scanWithUltrasonic();
            lastLineScan = millis();
        }

        return;
    }

    // Line following logic using PID controller
    float error = 0;

    switch (linePosition) {
        case -1:  // Line is to the left
            error = -1.0;
            break;
        case 0:   // Centered
            error = 0;
            break;
        case 1:   // Line is to the right
            error = 1.0;
            break;
    }

    // Calculate PID correction
    float correction = linePID.calculate(error);

    // Apply correction to motor speeds
    int leftSpeed = BASE_SPEED - (int)correction;
    int rightSpeed = BASE_SPEED + (int)correction;

    // Limit speeds
    leftSpeed = constrain(leftSpeed, 50, 250);
    rightSpeed = constrain(rightSpeed, 50, 250);

    setMotorSpeeds(leftSpeed, rightSpeed);

    // Check for obstacles while following line
    int distance = readUltrasonic();
    if (distance < SAFE_DISTANCE) {
        stopMotors();
        linePID.reset();
        currentState = OBSTACLE_AVOIDANCE;
    }
}

void handleStoppedState() {
    stopMotors();
    digitalWrite(STATUS_LED, HIGH);

    // Emergency stop activated, require manual reset
    // In a real implementation, you would need a way to resume
}

void handleEmergencyState() {
    stopMotors();

    // Flash LED and sound alarm
    digitalWrite(STATUS_LED, millis() % 500 < 250);
    if (millis() % 1000 < 500) {
        tone(BUZZER_PIN, 2000, 100);
    }

    // Emergency stop - require manual intervention
    Serial.println("EMERGENCY! Robot stopped!");
}

void updateRobotStatus() {
    static unsigned long lastStatusUpdate = 0;

    if (millis() - lastStatusUpdate > 5000) {  // Update every 5 seconds
        Serial.printf("State: %d, Speed: %d, Obstacles: %d, Turns: %d\n",
                     currentState, currentSpeed, obstaclesAvoided, turnsMade);
        lastStatusUpdate = millis();
    }
}

void checkManualCommands() {
    // Check for serial commands
    if (Serial.available()) {
        String command = Serial.readStringUntil('\n');
        command.trim();

        if (command == "STOP") {
            currentState = STOPPED;
            Serial.println("Manual stop command received");
        } else if (command == "START") {
            if (currentState == STOPPED) {
                currentState = LINE_FOLLOWING;
                Serial.println("Manual start command received");
            }
        } else if (command == "FORWARD") {
            currentState = MOVING_FORWARD;
            Serial.println("Manual forward command received");
        } else if (command == "LEFT") {
            turnLeft(90);
            Serial.println("Manual left turn command received");
        } else if (command == "RIGHT") {
            turnRight(90);
            Serial.println("Manual right turn command received");
        } else if (command == "SCAN") {
            scanWithUltrasonic();
            Serial.println("Manual scan command received");
        } else if (command == "STATUS") {
            printRobotStatus();
        } else if (command == "HELP") {
            printHelp();
        }
    }
}

void printRobotStatus() {
    Serial.println("\n=== Robot Status ===");
    Serial.printf("Current State: %d\n", currentState);
    Serial.printf("Current Speed: %d\n", currentSpeed);
    Serial.printf("Motors Enabled: %s\n", motorsEnabled ? "Yes" : "No");
    Serial.printf("Ultrasonic Distance: %d cm\n", readUltrasonic());
    Serial.printf("Line Detected: %s\n", lineDetected ? "Yes" : "No");
    Serial.printf("Line Position: %d\n", linePosition);
    Serial.printf("Total Obstacles Avoided: %d\n", obstaclesAvoided);
    Serial.printf("Total Turns Made: %d\n", turnsMade);
    Serial.printf("Operation Time: %lu ms\n", millis());
    Serial.printf("Free Memory: %d bytes\n", getFreeMemory());
    Serial.println("========================");
}

void printHelp() {
    Serial.println("\n=== Robot Commands ===");
    Serial.println("STOP    - Emergency stop");
    Serial.println("START   - Resume operation");
    Serial.println("FORWARD - Move forward");
    Serial.println("LEFT    - Turn left 90°");
    Serial.println("RIGHT   - Turn right 90°");
    Serial.println("SCAN    - Scan surroundings");
    Serial.println("STATUS  - Show robot status");
    Serial.println("HELP    - Show this help");
    Serial.println("==================");
}

int getFreeMemory() {
    #ifdef ARDUINO_ARCH_ESP32
        return ESP.getFreeHeap();
    #else
        extern int __heap_start, *__brkval;
        int v;
        return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
    #endif
}

// Utility functions
void beep(int frequency, int duration) {
    tone(BUZZER_PIN, frequency, duration);
}

void blinkLED(int count, int duration) {
    for (int i = 0; i < count; i++) {
        digitalWrite(STATUS_LED, HIGH);
        delay(duration);
        digitalWrite(STATUS_LED, LOW);
        delay(duration);
    }
}

// Emergency and recovery functions
void emergencyStop() {
    currentState = EMERGENCY;
    Serial.println("Emergency stop activated!");
}

void resetRobot() {
    Serial.println("Resetting robot systems...");

    // Reset variables
    currentSpeed = BASE_SPEED;
    linePID.reset();
    obstaclesAvoided = 0;
    turnsMade = 0;
    lineDetected = false;
    linePosition = 2;

    // Reset servos
    ultrasonicServo.write(90);

    // Reset motors
    stopMotors();

    delay(1000);

    // Start in safe state
    currentState = IDLE;

    Serial.println("Robot reset completed");
}

// Arduino Real-Time System Framework
// Complete RTOS-like implementation for Arduino with task scheduling and interrupt management
// Compatible with Arduino Mega/Uno (with limitations)

#include <Arduino.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <util/atomic.h>

// Real-time system configuration
#define MAX_TASKS 8
#define MAX_INTERRUPTS 16
#define SYSTEM_TICK_INTERVAL_MS 1
#define TICK_COUNT_TO_MS(tick) ((tick) * SYSTEM_TICK_INTERVAL_MS)
#define MS_TO_TICK_COUNT(ms) ((ms) / SYSTEM_TICK_INTERVAL_MS)

// Task priorities
#define PRIORITY_IDLE 0
#define PRIORITY_LOW 1
#define PRIORITY_NORMAL 2
#define PRIORITY_HIGH 3
#define PRIORITY_CRITICAL 4

// Task states
enum TaskState {
    TASK_READY,
    TASK_RUNNING,
    TASK_WAITING,
    TASK_SUSPENDED,
    TASK_TERMINATED
};

// Task control block structure
struct TaskControlBlock {
    void (*taskFunction)(void);
    char name[16];
    TaskState state;
    uint8_t priority;
    uint32_t delayTicks;
    uint32_t deadlineTicks;
    uint32_t executionCount;
    uint32_t maxExecutionTime;
    uint32_t totalExecutionTime;
    uint32_t lastStartTime;
    uint32_t lastDeadlineMiss;
    bool periodic;
    uint32_t periodTicks;
    uint32_t stackPointer;  // Simulated stack pointer for Arduino Uno
};

// Interrupt handler structure
struct InterruptHandler {
    void (*handler)(void);
    char name[16];
    uint8_t priority;
    uint32_t count;
    bool enabled;
};

// Global system variables
TaskControlBlock tasks[MAX_TASKS];
InterruptHandler interrupts[MAX_INTERRUPTS];
volatile uint32_t systemTickCount = 0;
volatile bool contextSwitchRequested = false;
uint8_t currentTaskIndex = 0;
uint8_t taskCount = 0;

// Performance monitoring
struct SystemStats {
    uint32_t totalTicks;
    uint32_t idleTicks;
    uint32_t contextSwitches;
    uint32_t missedDeadlines;
    float cpuUtilization;
    uint32_t maxInterruptLatency;
    uint32_t totalInterruptTime;
};

SystemStats stats;

// Task scheduling
volatile bool schedulerRunning = false;

// Initialize the real-time system
void initializeRTS() {
    Serial.begin(115200);
    delay(2000);

    Serial.println("=== Arduino Real-Time System ===");
    Serial.println("Initializing RTOS framework...");

    // Initialize system
    memset(&tasks, 0, sizeof(tasks));
    memset(&interrupts, 0, sizeof(interrupts));
    memset(&stats, 0, sizeof(stats));

    // Configure timer interrupt for system tick
    setupSystemTimer();

    // Enable global interrupts
    sei();

    // Create system monitor task
    createTask("SystemMonitor", PRIORITY_LOW, systemMonitorTask, 1000, false, 0);

    schedulerRunning = true;

    Serial.println("RTOS framework initialized");
    Serial.printf("System tick interval: %d ms\n", SYSTEM_TICK_INTERVAL_MS);
    Serial.printf("Maximum tasks: %d\n", MAX_TASKS);
}

void setupSystemTimer() {
    // Configure Timer1 for system tick (Arduino Uno/Mega)
    noInterrupts();

    TCCR1A = 0;  // Normal mode
    TCCR1B = 0;
    TCCR1C = 0;

    // Set CTC mode
    TCCR1B |= (1 << WGM12);

    // Set prescaler to 64
    TCCR1B |= (1 << CS11) | (1 << CS10);

    // Set compare value for 1ms tick (16MHz / 64 / 1000 = 250)
    OCR1A = 250;

    // Enable compare match interrupt
    TIMSK1 |= (1 << OCIE1A);

    interrupts();
}

// System timer interrupt service routine
ISR(TIMER1_COMPA_vect) {
    systemTickCount++;

    // Check for task delays and deadlines
    checkTaskDeadlines();

    // Request context switch if needed
    contextSwitchRequested = true;
}

// Task management functions
int createTask(const char* name, uint8_t priority, void (*taskFunction)(void),
                uint32_t period, bool periodic, uint32_t initialDelay) {
    if (taskCount >= MAX_TASKS) {
        return -1;  // No free task slots
    }

    // Find free task slot
    for (int i = 0; i < MAX_TASKS; i++) {
        if (tasks[i].taskFunction == NULL) {
            // Initialize task control block
            strncpy(tasks[i].name, name, sizeof(tasks[i].name) - 1);
            tasks[i].name[sizeof(tasks[i].name) - 1] = '\0';
            tasks[i].taskFunction = taskFunction;
            tasks[i].state = TASK_READY;
            tasks[i].priority = priority;
            tasks[i].delayTicks = MS_TO_TICK_COUNT(initialDelay);
            tasks[i].deadlineTicks = 0;
            tasks[i].executionCount = 0;
            tasks[i].maxExecutionTime = 0;
            tasks[i].totalExecutionTime = 0;
            tasks[i].lastStartTime = 0;
            tasks[i].lastDeadlineMiss = 0;
            tasks[i].periodic = periodic;
            tasks[i].periodTicks = MS_TO_TICK_COUNT(period);
            tasks[i].stackPointer = 0;  // Simplified for Arduino

            taskCount++;
            Serial.printf("Task created: %s (Priority: %d)\n", name, priority);
            return i;
        }
    }

    return -1;  // No free slots
}

void terminateTask(int taskIndex) {
    if (taskIndex >= 0 && taskIndex < MAX_TASKS && tasks[taskIndex].taskFunction != NULL) {
        tasks[taskIndex].state = TASK_TERMINATED;
        tasks[taskIndex].taskFunction = NULL;
        taskCount--;
        Serial.printf("Task terminated: %s\n", tasks[taskIndex].name);
    }
}

void suspendTask(int taskIndex) {
    if (taskIndex >= 0 && taskIndex < MAX_TASKS) {
        tasks[taskIndex].state = TASK_SUSPENDED;
        Serial.printf("Task suspended: %s\n", tasks[taskIndex].name);
    }
}

void resumeTask(int taskIndex) {
    if (taskIndex >= 0 && taskIndex < MAX_TASKS) {
        if (tasks[taskIndex].state == TASK_SUSPENDED) {
            tasks[taskIndex].state = TASK_READY;
            Serial.printf("Task resumed: %s\n", tasks[taskIndex].name);
        }
    }
}

void setTaskPriority(int taskIndex, uint8_t priority) {
    if (taskIndex >= 0 && taskIndex < MAX_TASKS) {
        tasks[taskIndex].priority = priority;
        Serial.printf("Task priority updated: %s -> %d\n",
                     tasks[taskIndex].name, priority);
    }
}

// Task delay functions
void taskDelay(uint32_t milliseconds) {
    int currentTask = currentTaskIndex;
    if (currentTask >= 0 && currentTask < MAX_TASKS) {
        tasks[currentTask].state = TASK_WAITING;
        tasks[currentTask].delayTicks = MS_TO_TICK_COUNT(milliseconds);
    }
}

void taskDelayUntil(uint32_t milliseconds) {
    int currentTask = currentTaskIndex;
    if (currentTask >= 0 && currentTask < MAX_TASKS) {
        tasks[currentTask].delayTicks = MS_TO_TICK_COUNT(milliseconds) - systemTickCount;
        tasks[currentTask].state = TASK_WAITING;
    }
}

// Scheduling functions
void schedule() {
    if (!schedulerRunning) return;

    int highestPriorityTask = -1;
    uint8_t highestPriority = 0;

    // Find highest priority ready task
    for (int i = 0; i < MAX_TASKS; i++) {
        if (tasks[i].state == TASK_READY && tasks[i].priority > highestPriority) {
            highestPriority = tasks[i].priority;
            highestPriorityTask = i;
        }
    }

    // Context switch if needed
    if (highestPriorityTask != -1 && highestPriorityTask != currentTaskIndex) {
        performContextSwitch(highestPriorityTask);
    }

    // Check for idle state
    if (highestPriorityTask == -1) {
        stats.idleTicks++;
        handleIdleState();
    }
}

void performContextSwitch(int newTaskIndex) {
    // Save current task state
    if (currentTaskIndex >= 0 && currentTaskIndex < MAX_TASKS) {
        tasks[currentTaskIndex].state = TASK_READY;
    }

    // Switch to new task
    currentTaskIndex = newTaskIndex;
    if (currentTaskIndex >= 0 && currentTaskIndex < MAX_TASKS) {
        tasks[currentTaskIndex].state = TASK_RUNNING;
        tasks[currentTaskIndex].lastStartTime = systemTickCount;
    }

    stats.contextSwitches++;
}

void checkTaskDeadlines() {
    for (int i = 0; i < MAX_TASKS; i++) {
        if (tasks[i].state == TASK_TERMINATED || tasks[i].taskFunction == NULL) {
            continue;
        }

        // Update delays
        if (tasks[i].state == TASK_WAITING) {
            if (tasks[i].delayTicks > 0) {
                tasks[i].delayTicks--;
            } else {
                tasks[i].state = TASK_READY;
            }
        }

        // Check deadlines for running tasks
        if (tasks[i].state == TASK_RUNNING && tasks[i].deadlineTicks > 0) {
            if (systemTickCount >= tasks[i].deadlineTicks) {
                tasks[i].lastDeadlineMiss = systemTickCount;
                stats.missedDeadlines++;
                Serial.printf("DEADLINE MISSED: %s\n", tasks[i].name);
            }
        }

        // Handle periodic tasks
        if (tasks[i].periodic && tasks[i].state == TASK_READY) {
            if (tasks[i].deadlineTicks <= systemTickCount) {
                tasks[i].deadlineTicks = systemTickCount + tasks[i].periodTicks;
            }
        }
    }
}

void handleIdleState() {
    // Enter low power mode when idle
    // This is a simplified implementation
    set_sleep_mode(SLEEP_MODE_IDLE);
    sleep_mode();
}

// Interrupt management
int registerInterrupt(const char* name, uint8_t priority, void (*handler)(void)) {
    for (int i = 0; i < MAX_INTERRUPTS; i++) {
        if (interrupts[i].handler == NULL) {
            strncpy(interrupts[i].name, name, sizeof(interrupts[i].name) - 1);
            interrupts[i].name[sizeof(interrupts[i].name) - 1] = '\0';
            interrupts[i].handler = handler;
            interrupts[i].priority = priority;
            interrupts[i].count = 0;
            interrupts[i].enabled = true;
            Serial.printf("Interrupt registered: %s (Priority: %d)\n", name, priority);
            return i;
        }
    }
    return -1;
}

void enableInterrupt(int interruptIndex) {
    if (interruptIndex >= 0 && interruptIndex < MAX_INTERRUPTS) {
        interrupts[interruptIndex].enabled = true;
        Serial.printf("Interrupt enabled: %s\n", interrupts[interruptIndex].name);
    }
}

void disableInterrupt(int interruptIndex) {
    if (interruptIndex >= 0 && interruptIndex < MAX_INTERRUPTS) {
        interrupts[interruptIndex].enabled = false;
        Serial.printf("Interrupt disabled: %s\n", interrupts[interruptIndex].name);
    }
}

// Sample tasks
void systemMonitorTask() {
    while (true) {
        // Calculate CPU utilization
        float utilization = ((float)(stats.totalTicks - stats.idleTicks) / stats.totalTicks) * 100.0;
        stats.cpuUtilization = utilization;

        // Print system statistics
        static unsigned long lastPrint = 0;
        if (systemTickCount - lastPrint >= TICK_COUNT_TO_MS(5000)) {  // Print every 5 seconds
            printSystemStats();
            lastPrint = systemTickCount;
        }

        taskDelay(1000);  // Monitor every second
    }
}

void blinkLEDTask() {
    const int ledPin = 13;
    pinMode(ledPin, OUTPUT);

    while (true) {
        digitalWrite(ledPin, HIGH);
        taskDelay(500);
        digitalWrite(ledPin, LOW);
        taskDelay(500);
    }
}

void readSensorTask() {
    while (true) {
        int sensorValue = analogRead(A0);
        float voltage = (sensorValue / 1023.0) * 5.0;

        Serial.printf("Sensor reading: %d (%.2fV)\n", sensorValue, voltage);

        taskDelay(2000);  // Read every 2 seconds
    }
}

void highPriorityTask() {
    uint32_t count = 0;

    while (true) {
        count++;
        Serial.printf("High priority task count: %lu\n", count);

        // This task should complete quickly
        taskDelay(100);
    }
}

void deadlineTask() {
    while (true) {
        uint32_t startTime = systemTickCount;

        // Simulate variable workload
        int workload = random(50, 200);
        delayMicroseconds(workload * 1000);

        uint32_t executionTime = systemTickCount - startTime;

        Serial.printf("Deadline task executed in %lu ms\n", TICK_COUNT_TO_MS(executionTime));

        // Wait until next period
        taskDelayUntil(1000);  // Execute every second with deadline
    }
}

// Performance monitoring functions
void printSystemStats() {
    Serial.println("\n=== System Statistics ===");
    Serial.printf("System Ticks: %lu\n", systemTickCount);
    Serial.printf("Total Ticks: %lu\n", stats.totalTicks);
    Serial.printf("Idle Ticks: %lu\n", stats.idleTicks);
    Serial.printf("CPU Utilization: %.2f%%\n", stats.cpuUtilization);
    Serial.printf("Context Switches: %lu\n", stats.contextSwitches);
    Serial.printf("Missed Deadlines: %lu\n", stats.missedDeadlines);
    Serial.printf("Active Tasks: %d\n", taskCount);
    Serial.printf("Free Memory: %d bytes\n", getFreeMemory());

    // Print task statistics
    Serial.println("\n--- Task Statistics ---");
    for (int i = 0; i < MAX_TASKS; i++) {
        if (tasks[i].taskFunction != NULL) {
            Serial.printf("%s: State=%d, Priority=%d, Count=%lu\n",
                         tasks[i].name, tasks[i].state, tasks[i].priority, tasks[i].executionCount);
        }
    }
    Serial.println("========================");
}

int getFreeMemory() {
    extern int __heap_start, *__brkval;
    int v;
    return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

// Main application
void setup() {
    initializeRTS();

    // Create example tasks
    createTask("BlinkLED", PRIORITY_NORMAL, blinkLEDTask, 0, false, 1000);
    createTask("ReadSensor", PRIORITY_NORMAL, readSensorTask, 0, false, 2000);
    createTask("HighPriority", PRIORITY_HIGH, highPriorityTask, 0, false, 100);
    createTask("DeadlineTask", PRIORITY_CRITICAL, deadlineTask, 1000, true, 500);

    // Create interrupts
    registerInterrupt("Timer2", PRIORITY_HIGH, timer2Handler);

    Serial.println("System ready, starting scheduler...");
}

void loop() {
    if (schedulerRunning) {
        stats.totalTicks++;

        schedule();

        // This loop provides the basic scheduling infrastructure
        // Real work is done in the scheduler and interrupt handlers
        delayMicroseconds(100);  // Small delay to prevent busy-waiting
    }
}

// Example interrupt handler
void timer2Handler() {
    // This would be triggered by Timer2 overflow or compare match
    // For demonstration purposes, we'll just increment a counter
    static uint32_t counter = 0;
    counter++;

    // Toggle pin to show interrupt is working
    static uint8_t pinState = 0;
    pinState = !pinState;
    digitalWrite(3, pinState);
}

// Utility functions for timing constraints
bool hasTimeRemaining(uint32_t startTime, uint32_t budget) {
    uint32_t elapsedTime = systemTickCount - startTime;
    return (elapsedTime < MS_TO_TICK_COUNT(budget));
}

uint32_t getRemainingTime(uint32_t startTime, uint32_t budget) {
    uint32_t elapsedTime = systemTickCount - startTime;
    uint32_t budgetTicks = MS_TO_TICK_COUNT(budget);

    if (elapsedTime >= budgetTicks) {
        return 0;
    }

    return TICK_COUNT_TO_MS(budgetTicks - elapsedTime);
}

// Time constraint checking macros
#define TIME_CONSTRAINT_START(budget) uint32_t __startTime = systemTickCount; uint32_t __budget = (budget)
#define CHECK_TIME_CONSTRAINT() if (!hasTimeRemaining(__startTime, __budget)) { Serial.println("TIME CONSTRAINT VIOLATED"); return; }
#define GET_REMAINING_TIME() getRemainingTime(__startTime, __budget)

// Example usage of time constraints
void timeConstrainedTask() {
    TIME_CONSTRAINT_START(10);  // 10ms budget

    // Some time-critical work
    CHECK_TIME_CONSTRAINT();

    // More work
    CHECK_TIME_CONSTRAINT();

    // Final work
    uint32_t remaining = GET_REMAINING_TIME();
    Serial.printf("Time remaining: %lu ms\n", remaining);
}

// Real-time message passing system
struct Message {
    uint8_t type;
    uint8_t source;
    uint8_t destination;
    uint8_t data[16];
    uint32_t timestamp;
};

#define MESSAGE_QUEUE_SIZE 10
Message messageQueue[MESSAGE_QUEUE_SIZE];
volatile int messageQueueHead = 0;
volatile int messageQueueTail = 0;

bool sendMessage(uint8_t type, uint8_t dest, uint8_t* data, uint8_t dataSize) {
    int nextTail = (messageQueueTail + 1) % MESSAGE_QUEUE_SIZE;

    ATOMIC_BLOCK(ATOMIC_RESTORESTATE);

    if (nextTail == messageQueueHead) {
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE);
        return false;  // Queue full
    }

    Message& msg = messageQueue[messageQueueTail];
    msg.type = type;
    msg.source = currentTaskIndex;
    msg.destination = dest;
    msg.timestamp = systemTickCount;

    if (dataSize > 16) dataSize = 16;
    memcpy(msg.data, data, dataSize);

    messageQueueTail = nextTail;

    ATOMIC_BLOCK(ATOMIC_RESTORESTATE);
    return true;
}

bool receiveMessage(uint8_t type, Message* msg) {
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE);

    if (messageQueueHead == messageQueueTail) {
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE);
        return false;  // Queue empty
    }

    if (messageQueue[messageQueueHead].type != type) {
        ATOMIC_BLOCK(ATOMIC_RESTORESTATE);
        return false;  // Wrong message type
    }

    *msg = messageQueue[messageQueueHead];
    messageQueueHead = (messageQueueHead + 1) % MESSAGE_QUEUE_SIZE;

    ATOMIC_BLOCK(ATOMIC_RESTORESTATE);
    return true;
}

// Example message passing tasks
void producerTask() {
    uint8_t counter = 0;

    while (true) {
        uint8_t data[] = {counter};
        sendMessage(1, 2, data, sizeof(data));

        counter++;
        taskDelay(1000);
    }
}

void consumerTask() {
    Message msg;

    while (true) {
        if (receiveMessage(1, &msg)) {
            Serial.printf("Received message from task %d: %d\n",
                         msg.source, msg.data[0]);
        }

        taskDelay(100);
    }
}

// Watchdog timer implementation
volatile uint32_t watchdogCounter = 0;
volatile bool watchdogReset = false;

void initializeWatchdog() {
    registerInterrupt("Watchdog", PRIORITY_CRITICAL, watchdogHandler);
}

void resetWatchdog() {
    watchdogCounter = 0;
}

void watchdogHandler() {
    if (!watchdogReset) {
        watchdogCounter++;

        // Trigger reset if watchdog not reset within timeout
        if (watchdogCounter > 1000) {  // 1 second timeout
            Serial.println("Watchdog timeout! Resetting...");
            asm volatile ("jmp 0");
        }
    }
}

// Use the watchdog in critical tasks
void criticalTask() {
    resetWatchdog();

    while (true) {
        // Critical work here
        resetWatchdog();
        taskDelay(100);
    }
}