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

#define uS_TO_S_FACTOR 1000000  // Conversion factor for microseconds to seconds
#define TIME_TO_SLEEP  52       // Time ESP32 will go to sleep, calculated from person code

#define MIN_DISTANCE 50.0  // Minimum distance in cm to consider the spot occupied

#define ECHO_PIN 13  // GPIO where the echo pin is connected
#define TRIG_PIN 12  // GPIO where the ultrasonic sensor's trigger pin is connected

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

esp_now_peer_info_t peerInfo;

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Failure");
}

void setup_ultrasonic_sensor() {
  pinMode(TRIG_PIN, OUTPUT); // Sets the trigPin as an Output
  pinMode(ECHO_PIN, INPUT); // Sets the echoPin as an Input
}

void setup_wifi() {
  WiFi.mode(WIFI_STA); // Sets WiFi mode to STA (station mode)
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  WiFi.setTxPower(WIFI_POWER_2dBm); // Adjust the WiFi transmit power if necessary
}

void register_peer() {
  memcpy(peerInfo.peer_addr, broadcastAddress, 6); // Copies the MAC address to the peer information
  peerInfo.channel = 0;  // Uses WiFi channel 0
  peerInfo.encrypt = false; // No encryption
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
}

void register_hooks() {
  esp_now_register_send_cb(OnDataSent); // Registers callback for when data is sent
}

void setup() {
  Serial.begin(115200); // Start the Serial communication
  setup_ultrasonic_sensor(); // Initialize ultrasonic sensor
  setup_wifi(); // Setup WiFi
  register_peer(); // Register the peer
  register_hooks(); // Register hooks

  // Enable timer to wake up from deep sleep based on the calculated duty cycle
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);

  Serial.println("Setup complete, device entering loop and will sleep after each cycle.");
}

float read_distance() {
  // Generate a 10 ms pulse for the TRIG_PIN
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10); // TODO: How does this delay affect the duty cycle?
  digitalWrite(TRIG_PIN, LOW);

  // Duration of ECHO_PIN being high is proportional to the measured distance
  long duration = pulseIn(ECHO_PIN, HIGH);

  float distance = duration / 58; // Calculate the distance

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  return distance;
}

void print_results(const char * statusMessage, long int reading_time, long int transmission_time)
{
  // Parking spot status
  Serial.print("Parking Spot Status: ");
  Serial.println(statusMessage);

  //Timing information
  Serial.println("Time spent reading the HC-SR04: " + reading_time);
  Serial.println("Time spent transmitting via wifi: " + transmission_time);
}

void loop() {


  long int initial_t, reading_time, transmission_time; //Defining the timing variables

  initial_t = millis(); //Get the initial ms count of this loop

  float distance = read_distance(); // Read the distance measured by the ultrasonic sensor

  reading_time = millis() - initial_t; // The difference gives us the time (in ms) the board spent readint the distance

  // Determine the parking spot's occupancy based on the distance
  const char* statusMessage = distance < MIN_DISTANCE ? "OCCUPIED" : "FREE";
  esp_now_send(broadcastAddress, (uint8_t *)statusMessage, strlen(statusMessage) + 1); // Send status message

  transmission_time = millis() - reading_time- initial_t;// The difference gives us the time (in ms) the board spent transmitting
  
  print_results(statusMessage, reading_time, transmission_time);

  // Enter deep sleep for a specified time
  Serial.println("Entering deep sleep mode");
  esp_deep_sleep_start();
}