#include <NewPing.h>

#define SONAR_TRIG_PIN_IN 26
#define SONAR_ECHO_PIN_IN 25
#define SONAR_TRIG_PIN_OUT 18
#define SONAR_ECHO_PIN_OUT 19
#define LED_WAIT_PIN 4
#define LED_ENTER_PIN 13

#define MAX_DISTANCE 200
#define MIN_DISTANCE 15
#define DEFAULT_DISTANCE 50

#define ITERATIONS 10
#define DELAY_BETWEEN_PINGS 40
#define DELAY_AFTER_CALIBRATION 1000

NewPing sonarIn(SONAR_TRIG_PIN_IN, SONAR_ECHO_PIN_IN, MAX_DISTANCE);
NewPing sonarOut(SONAR_TRIG_PIN_OUT, SONAR_ECHO_PIN_OUT, MAX_DISTANCE);

int people_count = 0;
int limit = 5;

int calibrateIn = 0;
int calibrateOut = 0;

bool prevInBlocked = false;
bool prevOutBlocked = false;


void setup() {
  Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.

  pinMode(LED_WAIT_PIN, OUTPUT);
  pinMode(LED_ENTER_PIN, OUTPUT);

  digitalWrite(LED_WAIT_PIN, HIGH);
  digitalWrite(LED_ENTER_PIN, HIGH); // Both LEDs are lit to alert user to ongoing calibration.

  Serial.println("Calibrating...");
  delay(DELAY_AFTER_CALIBRATION);

  for (int a = 0; a < ITERATIONS; a++) {
    delay(50);
    calibrateIn += sonarIn.ping_cm();
    delay(50);
    calibrateOut += sonarOut.ping_cm();
    delay(200);
  }

  calibrateIn = 0.75 * calibrateIn / ITERATIONS; // The threshold is set at 75% of the average of these readings. This should prevent the system counting people if it is knocked.
  calibrateOut = 0.75 * calibrateOut / ITERATIONS;

  if (calibrateIn > MAX_DISTANCE || calibrateIn < MIN_DISTANCE) { // If the calibration gave a reading outside of sensible bounds, then the default is used
    calibrateIn = DEFAULT_DISTANCE;
  }
  if (calibrateOut > MAX_DISTANCE || calibrateOut < MIN_DISTANCE) {
    calibrateOut = DEFAULT_DISTANCE;
  }

  Serial.print("Entry threshold set to: ");
  Serial.println(calibrateIn);
  Serial.print("Exit threshold set to: ");
  Serial.println(calibrateOut);

  digitalWrite(LED_WAIT_PIN, LOW);
  digitalWrite(LED_ENTER_PIN, LOW); // Both LEDs are off to alert user that calibration has finished.

  delay(DELAY_AFTER_CALIBRATION);
}

void loop() {
  int distanceIn = sonarIn.ping_cm();
  delay(DELAY_BETWEEN_PINGS); // Wait 40 milliseconds between pings. 29ms should be the shortest delay between pings.
  int distanceOut = sonarOut.ping_cm();
  delay(DELAY_BETWEEN_PINGS);

  if (distanceIn < calibrateIn && distanceIn > 0) {
  // If closer than wall/calibrated object (person is present) && throw out zero readings
  if (!prevInBlocked) {
    if (people_count < limit) {
      Serial.print("People inside room: ");
      Serial.println(people_count);
      people_count++;
    } else if (people_count == limit) {
      Serial.print("Maximum capacity reached! ");
      Serial.print(people_count);
      Serial.println(" people inside.");
      people_count++;
    } else if (people_count > limit) {
      Serial.print("Warning: Overcapacity! ");
      Serial.print(people_count);
      Serial.println(" people inside. ");
      people_count++;
    }
  }
  prevInBlocked = true;
} else {
  prevInBlocked = false;
}
  if (distanceOut < calibrateOut && distanceOut > 0) {
    if (!prevOutBlocked && people_count > 0) {
      people_count--; // Decrease count by one
      Serial.print("People inside room: ");
      Serial.println(people_count);
    }
    prevOutBlocked = true;
  } else {
    prevOutBlocked = false;
  }
  if (people_count > 0 && people_count < 5) { // If there are people present, then turn on the LED
    digitalWrite(LED_ENTER_PIN, HIGH);
  } else {
    digitalWrite(LED_ENTER_PIN, LOW);
  }
  if (people_count >= limit) { // If the number of people is greater than the limit, then turn on the LED
    digitalWrite(LED_WAIT_PIN, HIGH);
  } else {
    digitalWrite(LED_WAIT_PIN, LOW);
  }
}

//THIS CODE WORKs

// const int trigPin1 = 4; // Trigger pin of sensor 1
// const int echoPin1 = 2; // Echo pin of sensor 1
// const int trigPin2 = 18; // Trigger pin of sensor 2
// const int echoPin2 = 19; // Echo pin of sensor 2
// const long maxDistance = 300; // Maximum distance in cm to measure
// const int minDistance = 10; // Minimum distance in cm for counting
// int people_count = 0; // Initialize people count to 0
// bool sensor1_triggered = false; // Initialize sensor1_triggered flag to false
// bool sensor2_triggered = false; // Initialize sensor2_triggered flag to false

// void setup() {
//   // Initialize serial communication
//   Serial.begin(9600);

//   // Initialize trigger and echo pins of sensors as inputs/outputs
//   pinMode(trigPin1, OUTPUT);
//   pinMode(echoPin1, INPUT);
//   pinMode(trigPin2, OUTPUT);
//   pinMode(echoPin2, INPUT);
// }

// void loop() {
//   // Trigger sensor 1
//   digitalWrite(trigPin1, LOW);
//   delayMicroseconds(2);
//   digitalWrite(trigPin1, HIGH);
//   delayMicroseconds(10);
//   digitalWrite(trigPin1, LOW);

//   // Measure the duration of the echo signal from sensor 1
//   long duration1 = pulseIn(echoPin1, HIGH);

//   // Calculate the distance from sensor 1
//   float distance1 = duration1 * 0.034 / 2;

//   // Trigger sensor 2
//   digitalWrite(trigPin2, LOW);
//   delayMicroseconds(2);
//   digitalWrite(trigPin2, HIGH);
//   delayMicroseconds(10);
//   digitalWrite(trigPin2, LOW);

//   // Measure the duration of the echo signal from sensor 2
//   long duration2 = pulseIn(echoPin2, HIGH);

//   // Calculate the distance from sensor 2
//   float distance2 = duration2 * 0.034 / 2;

//   // Check if both sensors detect an object within range
//   if (distance1 < maxDistance && distance2 < maxDistance) {
//     // Calculate the distance difference between the two sensors
//     float distance_diff = abs(distance1 - distance2);

//     // Check if the object has moved a certain distance within a certain time frame
//     if (distance_diff >= minDistance) {
//       if (sensor1_triggered && !sensor2_triggered) {
//         // Increment people count and reset the sensor1_triggered flag
//         people_count++;
//         sensor1_triggered = false;
//         // Print the "entered room" message and the people count
//         Serial.println("Person entered the room. People count: " + String(people_count));
//       } else if (sensor2_triggered && !sensor1_triggered) {
//         // Decrement people count and reset the sensor2_triggered flag
//         people_count--;
//         sensor2_triggered = false;
//         // Print the "left room" message and the people count
//         Serial.println("Person left the room. People count: " + String(people_count));
//       }
//     }
//   } else {
//     if (distance1 < maxDistance && !sensor1_triggered) {
//       // Set the sensor1_triggered flag
//       sensor1_triggered = true;
//     } else if (distance2 < maxDistance && !sensor2_triggered) {
//       // Set the sensor2_triggered flag
//       sensor2_triggered = true;
//     }
//   }
//   // Simulate a person entering or leaving the room randomly (comment/uncomment as desired)
//   if (random(0, 2) == 0 && people_count > 0) {
//     // Simulate a person leaving the room
//     people_count--;
//     Serial.println("Person left the room. People count: " + String(people_count));
//   } else if (random(0, 2) == 1) {
//     // Simulate a person entering the room
//     people_count++;
//     Serial.println("Person entered the room. People count: " + String(people_count));
//   }
//   delay(random(2000, 5000)); // Wait for a random time between 2 and 5 seconds before simulating again
// }