#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;

bool inActive = false;
bool outActive = false;

bool inFirst = false;
bool outFirst = false;

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

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

  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);

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

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

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

  digitalWrite(4, inActive);
  digitalWrite(5, outActive);

  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;
      if (!inActive) {
        inActive = true;
        if (!outActive) inFirst = true;
      } else {
        inActive = false;
      }

    }
    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;
      // }
      if (!outActive) {
        outActive = true;
        if (!inActive) outFirst = true;
      } else {
        outActive = false;
      }

    }
    previousOutSensorState = currentOutSensorState;
  }


  if (inFirst && inActive && outActive) {
    counter = counter + 1;
    resetStates()

  } else if (outFirst && outActive && inActive) {
    if (counter > 0) {
      counter = counter - 1;
    }
    resetStates()
  }

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

void resetStates() {
  inActive = false;
  outActive = false;
  inFirst = false;
  outFirst = false;
}