#include<Wire.h>
#include<WiFi.h>
#include <ESP32Servo.h>
#include<esp_now.h>
#include <SD.h>
//MAC  receiver 
uint8_t broadcastAddress[] = {0x8C,0xAA, 0xB5, 0x84, 0xFB, 0x90};
esp_now_peer_info_t peerInfo;
const int TRIG_PIN_1 = 22;
const int ECHO_PIN_1 = 23;
const int TRIG_PIN_2 = 19;
const int ECHO_PIN_2 = 18;
const int SERVO_PIN = 18;
const float MINIMUM_DISTANCE = 5.0;
Servo servo;
unsigned long deepSleepDuration = 27000; // 27 seconds in milliseconds
unsigned long readSensorDuration = 5000; // 5 seconds in milliseconds
unsigned long sendTxDuration = 2000;     // 2 seconds in milliseconds

unsigned long deepSleepEnergy = 2000;    // Energy consumed during deep sleep (in joules)
unsigned long readSensorEnergy = 1500;   // Energy consumed during sensor reading (in joules)
unsigned long sendTxEnergy = 777;        // Energy consumed during transmission (in joules)

unsigned long initialBatteryEnergy = 4277;    // Initial battery energy in joules
unsigned long remainingEnergy = initialBatteryEnergy;// Remaining Energy in joules

void logToFile(String filename, unsigned long time, unsigned long energy) {
  File file = SD.open(filename, FILE_WRITE);
  if (file) {
    file.print(time);
    file.print(",");
    file.println(energy);
    file.close();
  }
}
void deepSleepWithEnergyLogging() {
  unsigned long startTime = millis();
  logToFile("deep_sleep.csv", startTime, deepSleepEnergy);
  remainingEnergy -= deepSleepEnergy;
  delay(100); // Simulate deep sleep by just delaying
}

void readSensorWithEnergyLogging() {
  unsigned long startTime = millis();
  float distance1 = measureDistance(TRIG_PIN_1, ECHO_PIN_1);
  float distance2 = measureDistance(TRIG_PIN_2, ECHO_PIN_2);
  logToFile("read_sensor.csv", startTime, readSensorEnergy);
  remainingEnergy -= readSensorEnergy;
}

void sendTxWithEnergyLogging() {
  unsigned long startTime = millis();
  logToFile("send_different_TX.csv", startTime, sendTxEnergy);
 remainingEnergy -= sendTxEnergy;
}
// Sending callback
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
void OnDataRecv(const uint8_t * mac, const uint8_t *data, int len) {
  Serial.print("Message received: ");
  char receivedString[len];
  memcpy(receivedString, data, len);
  Serial.println(String(receivedString));

}
void setup() {
  
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  esp_now_init();
   //send callback
  esp_now_register_send_cb(OnDataSent);
  //receive callback
  esp_now_register_recv_cb(OnDataRecv);

  // Peer Registration
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  // Add peer (Is possible to register multiple peers)
  esp_now_add_peer(&peerInfo);

  pinMode(TRIG_PIN_1,OUTPUT);
  pinMode(ECHO_PIN_1,INPUT);
  pinMode(TRIG_PIN_2,OUTPUT);
  pinMode(ECHO_PIN_2,INPUT);
  servo.attach(SERVO_PIN);
    
}

float measureDistance(int triggerPin, int echoPin){
  digitalWrite(triggerPin,LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin,HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin,LOW);
  return pulseIn(echoPin,HIGH)/58.2f;
}
bool checkParkingSpotOccupied(){
  float distance1 = measureDistance(TRIG_PIN_1,ECHO_PIN_1);
  float distance2 = measureDistance(TRIG_PIN_2,ECHO_PIN_2);
  Serial.print("Distance:- \n");
  Serial.print(distance1);
  Serial.print(", \n");
  Serial.print(distance2);
  Serial.print("\n");

  bool occupied = false;
  if (distance1> MINIMUM_DISTANCE||distance2 > MINIMUM_DISTANCE)
{
  occupied = true;
  }
  else{
    occupied = false;
  }
  return occupied;
}
void loop() {
  while (!Serial.available()); // Wait for input
  String message = Serial.readStringUntil('\n');
    esp_now_send(broadcastAddress, (uint8_t*)message.c_str(), message.length() + 1);

 bool occupied = checkParkingSpotOccupied();
 if(occupied){
  Serial.print("OCCUPIED:(\n");
  servo.write(0);
 }else{
  Serial.print("FREE:)");
  servo.write(90);
 }
  unsigned long currentTime = millis();
  if (currentTime % (deepSleepDuration + readSensorDuration + sendTxDuration) < deepSleepDuration) {
    deepSleepWithEnergyLogging();
  } else if (currentTime % (deepSleepDuration + readSensorDuration + sendTxDuration) < deepSleepDuration + readSensorDuration) {
    readSensorWithEnergyLogging();
  } else {
    sendTxWithEnergyLogging();
  }
if(remainingEnergy = 0){
  Serial.println("\nBattery depleted. Exiting");
  return;
  while(!Serial.available());//wait for input
  Serial.println("\n Battery not available !");
  esp_now_send(broadcastAddress,(uint8_t*)message.c_str(),message.length()+1);
}
  delay(500); // this speeds up the simulation
}