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

int Trig_pin = 5;
int Echo_pin = 18;
long duration;
float Speed_of_sound = 0.034;
float dist_in_cm;
#define DEEP_SLEEP_TIME 50e6 // X seconds that found by given formula from the challenge. My person code is 10867695. X=95%50 +5=50

// ESP-NOW Peer Address (Sink Node)

uint8_t broadcastAddress[] = {0x8C, 0xAA, 0xB5, 0x84, 0xFB, 0x90};

// Message Structure
typedef struct struct_message {
    char status[10]; // "OCCUPIED" or "FREE"
} struct_message;

// Create Message
struct_message parkingStatus;

// Callback function when message is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
    Serial.print("ESP-NOW Send Status: ");
    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
}

// Callback functàon recieve
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int len ) {
    Serial.print("Message received: ");
    char receivedString[len];
    memcpy(receivedString, data, len);
    Serial.println(String(receivedString));

}

void setup() {
    Serial.begin(115200);
    pinMode(Trig_pin, OUTPUT);
    pinMode(Echo_pin, INPUT);
    
    // Initialize ESP-NOW
    WiFi.mode(WIFI_STA);
    if (esp_now_init() != ESP_OK) {
        Serial.println("Error initializing ESP-NOW");
        return;
    }

    // Register peer
    esp_now_peer_info_t peerInfo;
    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;

    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
        Serial.println("Failed to add peer");
        return;
    }

    // Register Send Callback
    esp_now_register_send_cb(OnDataSent);
}

void loop() {

    unsigned long start_time = millis();  // Start time of wake-up
    // Measure Distance
    digitalWrite(Trig_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(Trig_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trig_pin, LOW);

    duration = pulseIn(Echo_pin, HIGH);
    dist_in_cm = duration * Speed_of_sound / 2;

    // Determine Parking Spot Status
    if (dist_in_cm <= 50) {
        strcpy(parkingStatus.status, "OCCUPIED");
    } else {
        strcpy(parkingStatus.status, "FREE");
    }

    Serial.print("Parking Status: ");
    Serial.println(parkingStatus.status);

    // Send Data via ESP-NOW
    esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&parkingStatus, sizeof(parkingStatus));

    if (result == ESP_OK) {
        Serial.println("Message Sent Successfully!");
    } else {
        Serial.println("Error Sending Message");
    }

    // Enter Deep Sleep Mode
    Serial.println("Going into Deep Sleep...");
    esp_deep_sleep(DEEP_SLEEP_TIME);

}