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

#define PIN_TRIG 12 //powered for 10uS to start sensor reading
#define PIN_ECHO 13 //read to obtain distance

#define MIN_DISTANCE 50 //[cm] - the space is occupied if the sensor reads a distance less than this

#define TIME_TO_SLEEP 18 //[s] - deep sleep period of the board
#define uS_TO_S 1000000

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

//variables for time accounting
unsigned long currentMicros = 0, prevMicros = 0;

//sending callback for ESP-NOW, prints whether the data was sent successfully
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 for ESP-NOW, prints the received message; only used for testing
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int len) {
  Serial.print("Message received: ");
  char received[len];
  memcpy(received, data, len);
  Serial.println(String(received));
}

int readDistance() {
  //trigger HC-SR04
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  //read the result and return the distance in cm
  int duration = pulseIn(PIN_ECHO, HIGH);
  return (duration / 58);
}

//measures and prints the elapsed time since this function was last called; printed time is in uS
void printElapsedTime(String msg) {
  currentMicros = micros();
  Serial.print(msg + ": ");
  Serial.println(currentMicros - prevMicros);
  prevMicros = currentMicros;
}

void setup() {
//PHASE 1 - initial setup

  //measure the starting time
  prevMicros = micros();

  //configure Serial port
  Serial.begin(115200);
  //configure HC-SR04 pins
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  printElapsedTime("Initial idle time");

//PHASE 2 - WiFi and ESP-NOW configuration

  //enable WiFi, set low transmission power
  WiFi.mode(WIFI_STA);
  WiFi.setTxPower(WIFI_POWER_2dBm);

  //configure ESP-NOW
  esp_now_init();
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);

  //register receiver
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  esp_now_add_peer(&peerInfo);

  printElapsedTime("Idle time with enabled wifi");

//PHASE 3 - distance measurement

  int distance = readDistance();

  printElapsedTime("Sensor reading time");

//PHASE 4 - message sending

  //determine if the space is occupied and send message
  String message = (distance < MIN_DISTANCE) ? "OCCUPIED" : "FREE";
  esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);

  //This delay is needed to give the node a chance to receive the message. Uncomment the line
  //to test the receiving callback.
  //delay(10);

  printElapsedTime("Transmission time");

//PHASE 5 - set up wakeup timer and start deep sleep

  WiFi.mode(WIFI_OFF);
  //set wakeup timer for deep sleep
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S);

  printElapsedTime("Idle up to deep sleep");
  
  esp_deep_sleep_start();
}

void loop() {}