// Include the required libraries
#include <WiFi.h> 
#include <esp_now.h>

// Define the broadcast address for ESP-NOW communication
uint8_t broadcastAddress[] = {0x8C, 0xAA, 0xB5, 0x84, 0xFB, 0x90};

// Define the structure to hold peer information for ESP-NOW
esp_now_peer_info_t peerInfo;

// Define the pins for the ultrasonic sensor
#define TRIG_PIN 26
#define ECHO_PIN 25

// Define the threshold value for ultrasonic sensor
const int maxDistance = 50;

// Define constants for converting microseconds to seconds and the time to sleep
#define uS_TO_S_FACTOR 1000000 
#define TIME_TO_SLEEP 7         // Timer duration in the deep sleep mode (seconds)

// Function prototypes
void setupIniziale();
void setupConnection();
float readDistance();
void sendESPMessage(float);
void goToSleep();
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status);
void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len);

// Initialize the ultrasonic sensor setup
void setupIniziale(){
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

// Set up the ESP-NOW connection
void setupConnection(){
  // Set WiFi mode to station
  WiFi.mode(WIFI_STA);
  //Serial.println("Accendo wifi");
  WiFi.setTxPower(WIFI_POWER_2dBm);
  delay(100);

  // Initialize ESP-NOW
  esp_now_init();
  
  // Register callback functions for data transmission and reception
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);

  // Register the peer (broadcast address, channel 0, not encrypted)
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
      Serial.println("Error initializing ESP-NOW");
  }
}

// Read distance from ultrasonic sensor
float readDistance(){
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

// Send ESP message based on sensor data
void sendESPMessage(float distance){
  String message = "";
  if(distance < maxDistance){
    message = "OCCUPIED";
  }
  else{
    message = "FREE";
  }
  // Send message using ESP-NOW
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);
  delay(5);
}

// Callback function for data transmission
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Ok" : "Error");
}

// Callback function for data reception
void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
  Serial.print("Message received: ");
  char receivedString[len];
  memcpy(receivedString, data, len);
  Serial.println(String(receivedString)); // Print the received message
}

// Put the device into deep sleep mode
void goToSleep(){
  // Turn off WiFi
  WiFi.mode(WIFI_OFF);
  delay(100);
  
  // Enable timer wakeup for deep sleep
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR); 

  // Enter deep sleep mode
  Serial.flush();
  esp_deep_sleep_start();
}


void setup() {
  // Initialize setup functions
  setupIniziale();  
  setupConnection();

  // Read distance from ultrasonic sensor
  float distance = readDistance();
  Serial.println("Distanza letta: " + String(distance));

  // Send ESP message based on distance
  sendESPMessage(distance);
  // Put the device into deep sleep mode
  goToSleep();
}

// Loop function - not used in this code
void loop() {
}