#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#define trigPinIn 6
#define echoPinIn 7
#define trigPinOut 10
#define echoPinOut 11

LiquidCrystal_I2C lcd(0x27, 16, 2);
int counter = 0;
int currentInSensorState = 0;
int previousInSensorState = 0;
int currentOutSensorState = 0;
int previousOutSensorState = 0;

void setup() {
  lcd.begin(16, 2);
  Serial.begin(9600);

  pinMode(trigPinIn, OUTPUT);
  pinMode(echoPinIn, INPUT);
  pinMode(trigPinOut, OUTPUT);
  pinMode(echoPinOut, INPUT);

  // Initialize
  digitalWrite(trigPinIn, LOW);
  digitalWrite(trigPinOut, LOW);

  lcd.init();
  lcd.backlight();
}

void loop() {
  // IN
  long durationIn, distanceIn, durationOut, distanceOut;

  digitalWrite(trigPinIn, HIGH);
  delay(100);
  digitalWrite(trigPinIn, LOW);

  durationIn = pulseIn(echoPinIn, HIGH);
  distanceIn = (durationIn / 2) / 29.1;

  if (distanceIn <= 10) {
    currentInSensorState = 1;
  } else {
    currentInSensorState = 0;
  }

  if (currentInSensorState != previousInSensorState) {
    if (currentInSensorState == 1) {
      counter = counter + 1;
    }
    previousInSensorState = currentInSensorState;
  }


  // OUT
  digitalWrite(trigPinOut, HIGH);
  delay(100);
  digitalWrite(trigPinOut, LOW);
  durationOut = pulseIn(echoPinOut, HIGH);
  distanceOut = (durationOut / 2) / 29.1;

  if (distanceOut <= 10) {
    currentOutSensorState = 1;
  } else {
    currentOutSensorState = 0;
  }

  if (currentOutSensorState != previousOutSensorState) {
    if (currentOutSensorState == 1) {
      if (counter > 0) {
        counter = counter - 1;
      }
    }
    previousOutSensorState = currentOutSensorState;
  }

  // >>> DEBUG >>>
  Serial.print("Current In: ");
  Serial.println(currentInSensorState);
  Serial.print("Previous In: ");
  Serial.println(previousInSensorState);
  Serial.print("Distance In:");
  Serial.println(distanceIn);
  Serial.print("Current Out: ");
  Serial.println(currentOutSensorState);
  Serial.print("Previous Out: ");
  Serial.println(previousOutSensorState);
  Serial.print("Distance Out:");
  Serial.println(distanceOut);
  Serial.print("Counter: ");
  Serial.println(counter);
  Serial.println("------------------");
  // <<< END DEBUG <<<

  lcd.setCursor(0, 0);
  lcd.print("Number of People");
  lcd.setCursor(7, 1);
  lcd.print(counter);
}