#define SENSOR_PIN_1 6 // buttons are used as placeholders
#define SENSOR_PIN_2 7
#define OUTPUT_PIN 8

enum State {
  IDLE,                   // waiting for first Sensor
  SENSOR1_TRIGGERED,      // Sensor 1 triggered, waiting for Sensor 2
  SENSOR2_TRIGGERED       // Sensor 2 triggered, waiting for Sensor 1
};
State currentState = IDLE;

int occupantCount = 0;

int prevSensor1State = LOW;
int prevSensor2State = LOW;
unsigned long lastDebounceMillis = 0;
unsigned long debounceDelay = 100;

void updateOutputState() {
  if (occupantCount > 0) {
    digitalWrite(OUTPUT_PIN, HIGH);
    Serial.println("[STATE] Output: ON");
  } else {
    digitalWrite(OUTPUT_PIN, LOW);
    Serial.println("[STATE] Output: OFF");
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial); // wait for serial connection

  Serial.println("[INFO] System initialized");
  Serial.print("[COUNT] Initial: ");
  Serial.println(occupantCount);
  Serial.println("------------------------------------");

  pinMode(SENSOR_PIN_1, INPUT);
  pinMode(SENSOR_PIN_2, INPUT);

  pinMode(OUTPUT_PIN, OUTPUT);
  updateOutputState(); // set initial output state
}

void loop() {
  int sensor1Reading = digitalRead(SENSOR_PIN_1);
  int sensor2Reading = digitalRead(SENSOR_PIN_2);

  // Sensor 1 trigger
  if (sensor1Reading == HIGH && prevSensor1State == LOW) {
    if ((millis() - lastDebounceMillis) > debounceDelay) {
      switch (currentState) {
        case IDLE:
          // start sequence (Sensor 1 first)
          currentState = SENSOR1_TRIGGERED;
          Serial.println("[SENSOR] Sensor 1 triggered - awaiting Sensor 2");
          break;
        case SENSOR2_TRIGGERED:
          // Sensor 2 -> Sensor 1 sequence = Exit
          if (occupantCount > 0) {
            occupantCount--;
            Serial.print("[EVENT] Exit detected - Count: ");
            Serial.println(occupantCount);
            updateOutputState();
          } else {
            Serial.println("[EVENT] Exit detected - count remains 0");
          }
          currentState = IDLE; // reset state
          break;
        case SENSOR1_TRIGGERED:
          // Sensor 1 -> Sensor 1 invalid, reset
          currentState = IDLE;
          break;
      }
      lastDebounceMillis = millis();
    }
  }

  // Sensor 2 trigger
  if (sensor2Reading == HIGH && prevSensor2State == LOW) {
    if ((millis() - lastDebounceMillis) > debounceDelay) {
      switch (currentState) {
        case IDLE:
          // start sequence (Sensor 2 first)
          currentState = SENSOR2_TRIGGERED;
          Serial.println("[SENSOR] Sensor 2 triggered - awaiting Sensor 1");
          break;
        case SENSOR1_TRIGGERED:
          // Sensor 1 -> Sensor 2 sequence = Entrance
          occupantCount++;
          Serial.print("[EVENT] Entry detected - Count: ");
          Serial.println(occupantCount);
          updateOutputState();
          currentState = IDLE; // reset state
          break;
        case SENSOR2_TRIGGERED:
          // Sensor 2 -> Sensor 2 invalid, reset
          currentState = IDLE;
          break;
      }
      lastDebounceMillis = millis();
    }
  }

  // store previous states
  prevSensor1State = sensor1Reading;
  prevSensor2State = sensor2Reading;
}