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

#define PIN_ECHO 4
#define PIN_TRIG 5
#define TRIG_DIST 50 // Minimum distance to consider the parking spot FREE
#define uS_TO_S_FACTOR 1000000
// Leader code = 106807[12] -> 12%50+5
#define X 17 // Deep Sleep period for one cycle (in seconds)

// ESP32 sink node address for simulation
uint8_t broadcastAddress[] = {0x8C, 0xAA, 0xB5, 0x84, 0xFB, 0x90};
esp_now_peer_info_t peerInfo;

// Function to read the distance from Ultrasonic Distance Sensor
// From docs -> https://docs.wokwi.com/parts/wokwi-hc-sr04
float readDistanceCM() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  int dist = pulseIn(PIN_ECHO, HIGH);
  return dist * 0.034 / 2;
}

// ESP-NOW: Sending callback
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status){
  //Serial.print("[THIS NODE] Parking Spot status update: ");
  //Serial.println(status == ESP_NOW_SEND_SUCCESS ? "SENT" : "FAILED" );
}

// ESP-NOW: Receiving callback
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len){
  //Serial.print("[PEER NODE] Parking Spot update received: ");
  char updatemsg[data_len];
  memcpy(updatemsg, data, data_len);
  //Serial.println(String(updatemsg));
}

// Function to set up communication, using ESP-NOW
unsigned long setup_ESPNOW(){
  unsigned long ts = micros(); // Timestamp: start of 'Wi-Fi on' state
  WiFi.mode(WIFI_STA); // Wi-Fi STAtion mode
  WiFi.setTxPower(WIFI_POWER_2dBm); // Minimum power to optimize consumption
 
  // Using return values of esp_now_xyz() to check everything is working accordingly
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error in ESP-NOW initialization");
    ESP.restart();
  }
  // Serial.println("ESP-NOW initialization successfully performed.");
  if (esp_now_register_send_cb(OnDataSent) != ESP_OK ||
    esp_now_register_recv_cb(OnDataRecv) != ESP_OK) {
    Serial.println("Error in callback functions registration.");
    ESP.restart();
  }
  //Serial.println("Callback functions for send and receive successfully registered.");
 
  // Peer registration
  memcpy(peerInfo.peer_addr, broadcastAddress, 6); // Copying the 6 chunks of broadcast address into peer
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if(esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("Error in peer registration");
    ESP.restart();
  }
  //Serial.println("Peer successfully registered.");
  return ts;
}

void setup() {
  unsigned long start_time = micros(); // Timestamp: start of execution (working time)
  Serial.begin(115200);
  // Setting the 2 pins to output (TRIG) and input (ECHO)
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);


  String spot_status; // String containing the message to be sent
  unsigned long ts_sensor_start = micros(); // Timestamp: start of 'Sensor reading' state
  spot_status = (readDistanceCM() < TRIG_DIST ? "OCCUPIED" : "FREE"); // Assign the correct value to the string to-be-sent
  // Computing and printing the period (us) in which the node is in 'Sensor reading' state
  unsigned long sensor_time = micros() - ts_sensor_start;
  Serial.println("Sensor used for " + String(sensor_time) + " us");
 
  // Sensor can collect data "on itself" without the need of setting up a connection
  unsigned long ts_wifi_start = setup_ESPNOW();
  // Computing and printing the period of "Transmission" state of the node; sending parking status through the previously set up connection
  unsigned long ts_send_start = micros();  
  esp_err_t check = esp_now_send(broadcastAddress, (uint8_t*)spot_status.c_str(), spot_status.length() + 1);
  if(check != ESP_OK){
    Serial.println("Error: message not sent.");
    ESP.restart();
  }
  unsigned long send_time = micros() - ts_send_start;
  Serial.println("Message transmission lasted " + String(send_time) + " us");  
  WiFi.mode(WIFI_OFF); // Wi-Fi is no more needed, turning it OFF to avoid waste of energy
  // Computing and printing the period (us) in which the sensor node used Wi-Fi (minus send time, in the calculations would be redundant)
  unsigned long wifi_time = micros() - ts_wifi_start - send_time;
  Serial.println("Message sent: " + spot_status);
  Serial.println("Wi-Fi connection used for " + String(wifi_time) + " us");


  // Final estimations of residence time in other states of the node and total work time
  unsigned long work_time = micros() - start_time;
  Serial.println("Total work time is " + String(work_time) + " us");
  unsigned long idle_time = work_time - sensor_time - wifi_time - send_time;
  Serial.println("Idle period is " + String(idle_time) + " us");  
  unsigned long sleep_time = X * uS_TO_S_FACTOR;
  Serial.println("Going to sleep now.");
  // Enable and enter in "Deep Sleep" state, for the specified time period
  esp_sleep_enable_timer_wakeup((uint64_t)sleep_time);
  Serial.println("ESP32 sleeps for " + String((float)sleep_time) + " us");
  Serial.flush();
  esp_deep_sleep_start();
  Serial.println("If you see this on the log, something's wrong!");
}

void loop() { // Won't reach this
  delay(100);
}