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

#define ECHO 32
#define TRIGGER 33
// MAC receiver
uint8_t broadcastAddress[] = {0x8C, 0xAA, 0xB5, 0x84, 0xFB, 0x90};

esp_now_peer_info_t peerInfo;

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

int get_distance() {
// Start a new measurement:     // to estimate the presence of a car: distance < 50cm
  digitalWrite(TRIGGER, HIGH);  // To change the distance while the simulation is running, click on
  delayMicroseconds(10);        // the HC-SR04 drawing in the diagram and use the slider to set the
  digitalWrite(TRIGGER, LOW);   // distance value. You can choose any value between 2cm and 400cm
// Read the result:
  int duration = pulseIn(ECHO, HIGH);
// Calculates the distance
  int distance = duration / 58;
  return distance; // centimeters
}


void setup() {
  Serial.begin(115200);
  pinMode(ECHO, INPUT);
  pinMode(TRIGGER, OUTPUT);
  WiFi.mode(WIFI_STA);
  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 (Is possible to register multiple peers)
  esp_now_add_peer(&peerInfo);
}

void loop() {
  delay(5000);

  Serial.println("Measured distance: " + String(get_distance()) + "cm");
  String message;
  if( get_distance() <= 50){
    message = "OCCUPIED";
  }
  else{
    message = "FREE";    
  }
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);

  /*while (!Serial.available()); // Wait for input
  String message = Serial.readStringUntil('\n');
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);*/
}