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

//#define SINK_SIMULATION


#ifdef SINK_SIMULATION
bool messageRecvd = false;
#endif

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;

/*
 * variable to track time between part of the code
 */
uint64_t lastTimeMeasurement = 0;

/*
 * When used print the time elapsed between that moment and the last time
 * the faction has been called
 * @param label [String] to track which time period I am printing
 */
inline void measureTime(String label) {
  uint64_t currTime = micros(); 
  Serial.print("for ["+label+"], \t\t the time is: ");
  Serial.print(currTime - lastTimeMeasurement);
  Serial.println(" micros.");
  lastTimeMeasurement = currTime;
}


/*
 * 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);
  
  measureTime("While Sending -> Until wifi off");
  esp_sleep_enable_timer_wakeup(SLEEP_TIME*SECONDS_TO_uS);
  measureTime("Until wifi off -> Going to sleep");
  Serial.flush();
  esp_deep_sleep_start();
} 

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

  measureTime("Setup Complete -> Read Sensor");

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


  measureTime("Read Sensor -> While Wifi ON");
  /*Sending sensor data*/
  esp_now_send(broadcastAddress, (uint8_t*) sensorData.c_str(), sensorData.length() + 1);

  measureTime("While Wifi ON -> While Sending");

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

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