#include <esp_now.h>
#include <WiFi.h>

// LED Pin Definitions
#define RED_LED_PIN   2
#define GREEN_LED_PIN 4

// Callback function: Triggered when data is received
void OnDataRecv(const esp_now_recv_info *recv_info, const uint8_t *data, int len) {
    const uint8_t *mac = recv_info->src_addr;  // Extract MAC address
    // Check if the data length is correct (sending end sends 4 bytes)
    if (len == 4) {
        // Parse data
        int occupancy = data[0];                // Status: 0=Free, 1=Occupied
        int distanceInt = (data[1] << 8) | data[2]; // Combine the integer part of distance
        float distance = distanceInt + data[3] / 100.0; // Combine the decimal part

        // Control LEDs
        digitalWrite(RED_LED_PIN, occupancy ? HIGH : LOW);
        digitalWrite(GREEN_LED_PIN, occupancy ? LOW : HIGH);

        // Print received information to serial monitor
        Serial.print("From: ");
        printMac(mac); // Print sender MAC address
        Serial.print(" Status: ");
        Serial.print(occupancy ? "Occupied" : "Free");
        Serial.print(", Distance: ");
        Serial.print(distance);
        Serial.println(" cm");
    } else {
        Serial.println("Error: Data length mismatch");
    }
}

// Helper function: Print MAC address
void printMac(const uint8_t *mac) {
    char macStr[18];
    snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
             mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
    Serial.print(macStr);
}

void setup() {
    Serial.begin(115200);
    
    // Initialize LED pins
    pinMode(RED_LED_PIN, OUTPUT);
    pinMode(GREEN_LED_PIN, OUTPUT);
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, LOW);

    // Initialize ESP-NOW
    WiFi.mode(WIFI_STA);
    if (esp_now_init() != ESP_OK) {
        Serial.println("ESP-NOW Initialization Failed!");
        return;
    }

    // Register the receive callback function
    esp_now_register_recv_cb(OnDataRecv);

    // Print the local MAC address (to configure the sender)
    Serial.print("Receiver MAC Address: ");
    Serial.println(WiFi.macAddress());
    Serial.println("Waiting for data...");
}

void loop() {
    // No tasks, all logic is handled in the callback function
}