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

#define SINK_SIMULATION

constexpr uint8_t PIN_TRIG = 25;
constexpr uint8_t PIN_ECHO = 33;
constexpr int THRESHOLD = 100; // 1 meter
constexpr uint64_t SLEEP_TIME = 52; // [s]
constexpr uint64_t SECONDS_TO_uS = 1000000;

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

esp_now_peer_info_t peerInfo;

#ifdef SINK_SIMULATION
bool messageRecvd = false;
#endif

/*
 * This method triggers the sensor's high frequency pulse
 */
inline void startReading() {
  // Start a new measurement
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
}

/*
 * This method allows for a sample to be read from the HC-SR Sensor
 * @return The distance from the target in cm. [Range: 0 <-> 400]
 */
inline int readSensor() {
  startReading();
  return pulseIn(PIN_ECHO, HIGH)/58;
}

/*
 * This method returns wether the parking slot is free or occupied based
 * on the sensor reading.
 * @return "FREE" if the distance is above the threshold
 * @return "OCCUPIED" if the distance is below or equal to the threshold
 */
inline String getSensorReading() {
  return readSensor() > THRESHOLD ? "FREE" : "OCCUPIED";
}

/*
 * This is the callback we can use to check if data was sent correctly by
 * the ESP-NOW protocol.
 */
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");
}

#ifdef SINK_SIMULATION
/*
 * This method is used for debugging and simulation purposes only.
 * This can allow us to simulate a working sink node.
 */
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(receivedString);
  messageRecvd = true;
}
#endif

/*
 * This method shuts down the ESP and puts it in a deep sleep mode.
 */
void shutdown() {
  WiFi.mode(WIFI_OFF);
  esp_sleep_enable_timer_wakeup(SLEEP_TIME*SECONDS_TO_uS);
  Serial.flush();
  esp_deep_sleep_start();
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Setupping PIN MODES");
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  
  /* Sensor Reading */
  String sensorData = getSensorReading();

  /*WIFI ENABLE*/
  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_register_send_cb(onDataSent);

#ifdef SINK_SIMULATION
  esp_now_register_recv_cb(onDataRecv);
#endif
  
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  esp_now_add_peer(&peerInfo);

  /*Sending sensor data*/
  esp_now_send(broadcastAddress, (uint8_t*) sensorData.c_str(), sensorData.length() + 1);

#ifdef SINK_SIMULATION
  messageRecvd = false;
#else
  shutdown();
#endif
}

void loop() {
#ifdef SINK_SIMULATION
  if (messageRecvd) {
    shutdown();
  }
#endif
}