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

#define PIN_ECHO 6
#define PIN_TRIG 7

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

esp_now_peer_info_t peerInfo;

#define uS_TO_S_FACTOR 1000000
#define TIME_TO_SLEEP  28 //Sleep for 28 seconds

int bootCount;

//Sending callback
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");
}

//Receiving Callback
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));
}

void setup() {

  Serial.begin(115200);

  WiFi.mode(WIFI_STA);

  //Increment bootCount
  ++bootCount;

  esp_now_init();
  //send callback
  esp_now_register_send_cb(OnDataSent);
  //receive callback
  esp_now_register_recv_cb(OnDataRecv);

  //Peer Registration
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  //Add peer
  esp_now_add_peer(&peerInfo);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  //Start a measurement
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  //read the result
  int duration = pulseIn(PIN_ECHO, HIGH);
  int distance = duration / 58;
  Serial.println("Distance: " + String(distance) + "cm");

  delay(1000);

  //Occupied if distance is less than 50 cm otherwise free
  String message = "";
  if (distance < 50){
    message = "OCCUPIED";
  }
  else{
    message = "FREE";
  }

  //Send message
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);

  //wake up every 28 seconds
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);
  //Wait for data to be transmitted
  Serial.flush();
  //Go in deep sleep
  esp_deep_sleep_start();
}

void loop() {
  
}