#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

/* ---------- OLED CONFIG ---------- */
#define OLED_W 128
#define OLED_H 64
Adafruit_SSD1306 oled(OLED_W, OLED_H, &Wire, -1);

/* ---------- ULTRASONIC SENSORS ---------- */
#define US1_TRIG 14
#define US1_ECHO 27
#define US2_TRIG 26
#define US2_ECHO 25

/* ---------- POTENTIOMETER ---------- */
#define CO2_POT 34

int personCounter = 0;
int activeSensor = 0;
unsigned long triggerTime = 0;

/* ---------- DISTANCE FUNCTION ---------- */
float getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long pulseTime = pulseIn(echoPin, HIGH, 30000);
  return pulseTime * 0.034 / 2;
}

void setup() {
  Serial.begin(115200);

  pinMode(US1_TRIG, OUTPUT);
  pinMode(US1_ECHO, INPUT);
  pinMode(US2_TRIG, OUTPUT);
  pinMode(US2_ECHO, INPUT);

  pinMode(CO2_POT, INPUT);

  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED init failed");
    while (true);
  }

  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
}

void loop() {
  delay(10);

  float distA = getDistance(US1_TRIG, US1_ECHO);
  float distB = getDistance(US2_TRIG, US2_ECHO);

  int co2ppm = map(analogRead(CO2_POT), 0, 4095, 400, 2000);
  unsigned long currentTime = millis();

  if (distA < 15 && activeSensor == 0) {
    activeSensor = 1;
    triggerTime = currentTime;
  }

  if (distB < 15 && activeSensor == 0) {
    activeSensor = 2;
    triggerTime = currentTime;
  }

  if (activeSensor == 1 && distB < 15 && currentTime - triggerTime < 2000) {
    personCounter++;
    activeSensor = 0;
  }
  else if (activeSensor == 2 && distA < 15 && currentTime - triggerTime < 2000) {
    if (personCounter > 0) personCounter--;
    activeSensor = 0;
  }

  if (currentTime - triggerTime > 2000) {
    activeSensor = 0;
  }

  oled.clearDisplay();
  oled.setCursor(0, 0);
  oled.print("CO2: ");
  oled.print(co2ppm);
  oled.println(" ppm");

  oled.print("People: ");
  oled.println(personCounter);

  oled.display();
  delay(200);
}