// Bidirectional occupant counter + relay control
// Sensor A = PIR on D4
// Sensor B = HC-SR04 TRIG->D3, ECHO->D2
// LCD I2C SDA->A4 SCL->A5 (auto-detect 0x27/0x3F)
// Saves counter to EEPROM
// Relay output on RELAY_PIN: turns ON when count > 0, OFF when count == 0

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

const int pirPin   = 4;   // Sensor A (digital PIR)
const int trigPin  = 3;   // Sensor B (HC-SR04 TRIG)
const int echoPin  = 2;   // Sensor B (HC-SR04 ECHO)

// Relay configuration
const int RELAY_PIN = 8;
const bool RELAY_ACTIVE_LOW = false; // Set true if relay module is active LOW.

// Tuning parameters
const unsigned long SEQUENCE_WINDOW_MS = 1500; // max time between A then B (or B then A)
const unsigned long LOCKOUT_MS = 2000;         // after a count, ignore new sequences briefly
const unsigned long DEBOUNCE_MS = 80;          // required stable active time to accept trigger
const int DIST_THRESHOLD_CM = 80;              // distance under this = "sensor B active"
const int ULTRA_SAMPLES = 3;                   // average samples to reduce noise
const unsigned long ULTRA_SAMPLE_DELAY_MS = 30;

enum SeqState { IDLE, A_TRIGGERED, B_TRIGGERED };
SeqState seqState = IDLE;

unsigned long stateTimestamp = 0;
unsigned long lastCountTime = 0;

int32_t countOccupants = 0;
bool lastRelayState = false;

const uint8_t lcdAddresses[] = { 0x27, 0x3F };
LiquidCrystal_I2C *lcd = nullptr;

// Read averaged distance in cm (returns -1 if out-of-range)
float readAverageDistanceCm() {
  long sum = 0;
  int valid = 0;
  for (int i = 0; i < ULTRA_SAMPLES; ++i) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    unsigned long dur = pulseIn(echoPin, HIGH, 30000UL); // 30ms timeout
    if (dur != 0) {
      float d = dur / 58.0; // cm
      sum += (long)(d * 10.0); // deci-cm
      valid++;
    }
    delay(ULTRA_SAMPLE_DELAY_MS);
  }
  if (valid == 0) return -1.0;
  float avg_deci = (float)sum / valid;
  return avg_deci / 10.0;
}

bool sensorAActive() {
  // PIR digital HIGH is active. Debounce requiring stable HIGH for DEBOUNCE_MS.
  static unsigned long a_high_since = 0;
  static bool a_was_high = false;
  bool val = digitalRead(pirPin) == HIGH;
  unsigned long now = millis();
  if (val) {
    if (!a_was_high) a_high_since = now;
    a_was_high = true;
    return (now - a_high_since >= DEBOUNCE_MS);
  } else {
    a_was_high = false;
    return false;
  }
}

bool sensorBActive() {
  // Ultrasonic: active if averaged distance <= threshold AND stable for DEBOUNCE_MS
  static unsigned long b_active_since = 0;
  static bool b_was_active = false;
  float d = readAverageDistanceCm();
  bool active = (d > 0 && d <= DIST_THRESHOLD_CM);
  unsigned long now = millis();
  if (active) {
    if (!b_was_active) b_active_since = now;
    b_was_active = true;
    return (now - b_active_since >= DEBOUNCE_MS);
  } else {
    b_was_active = false;
    return false;
  }
}

void saveCountToEEPROM() {
  EEPROM.put(0, countOccupants);
}

void loadCountFromEEPROM() {
  EEPROM.get(0, countOccupants);
  if (countOccupants < 0) countOccupants = 0;
}

// Set the relay hardware state
void setRelayHardware(bool on) {
  // Map logical ON -> physical pin value depending on RELAY_ACTIVE_LOW
  bool pinVal = RELAY_ACTIVE_LOW ? !on : on;
  digitalWrite(RELAY_PIN, pinVal);
}

// High-level relay setter (avoids redundant writes, updates lastRelayState)
void updateRelayForCount() {
  bool shouldBeOn = (countOccupants > 0);
  if (shouldBeOn != lastRelayState) {
    setRelayHardware(shouldBeOn);
    lastRelayState = shouldBeOn;
    Serial.print("Relay ");
    Serial.println(shouldBeOn ? "ON" : "OFF");
  }
}

void setupLCD() {
  Wire.begin();
  uint8_t foundAddr = 0;
  for (uint8_t i = 0; i < sizeof(lcdAddresses); ++i) {
    Wire.beginTransmission(lcdAddresses[i]);
    if (Wire.endTransmission() == 0) {
      foundAddr = lcdAddresses[i];
      break;
    }
  }
  if (foundAddr) {
    lcd = new LiquidCrystal_I2C(foundAddr, 16, 2);
    lcd->init();
    lcd->backlight();
    lcd->clear();
  }
}

// Unified display update — shows count and relay state + short message
void updateDisplay(const char *msg = "Monitoring...") {
  if (!lcd) return;
  char line1[17];
  char line2[17];
  snprintf(line1, sizeof(line1), "Count: %ld", (long)countOccupants);
  snprintf(line2, sizeof(line2), "%s %s",
           (lastRelayState ? "Relay:ON " : "Relay:OFF"),
           msg);
  // ensure lines fit 16 chars
  line1[16] = '\0';
  line2[16] = '\0';
  lcd->setCursor(0,0);
  lcd->print("                ");
  lcd->setCursor(0,0);
  lcd->print(line1);
  lcd->setCursor(0,1);
  lcd->print("                ");
  lcd->setCursor(0,1);
  lcd->print(line2);
}


void handleSequenceAthenB() {
  // A then B => increment
  countOccupants++;
  if (countOccupants < 0) countOccupants = 0;
  saveCountToEEPROM();
  lastCountTime = millis();
  seqState = IDLE;
  Serial.print("IN  count=");
  Serial.println((long)countOccupants);

  updateRelayForCount();
  updateDisplay("IN  (A->B)");
}

void handleSequenceBthenA() {
  // B then A => decrement
  if (countOccupants > 0) countOccupants--;
  saveCountToEEPROM();
  lastCountTime = millis();
  seqState = IDLE;
  Serial.print("OUT count=");
  Serial.println((long)countOccupants);

  updateRelayForCount();
  updateDisplay("OUT (B->A)");
}


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

  pinMode(pirPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(RELAY_PIN, OUTPUT);
  // Initialize relay to a safe state (OFF)
  setRelayHardware(false);
  lastRelayState = false;

  setupLCD();
  loadCountFromEEPROM();

  // ensure relay matches loaded count
  updateRelayForCount();

  // Initial display
  updateDisplay("Ready");
  Serial.println("Bidirectional counter + relay started");
  Serial.print("Initial count: ");
  Serial.println((long)countOccupants);
  Serial.print("Initial relay: ");
  Serial.println(lastRelayState ? "ON" : "OFF");
}

void loop() {
  unsigned long now = millis();

  // Short lockout after a count to avoid double counting (same crossing)
  if (now - lastCountTime < LOCKOUT_MS) {
    static unsigned long lastLCDupdate = 0;
    if (now - lastLCDupdate > 500) {
      updateDisplay("Lockout...");
      lastLCDupdate = now;
    }
    delay(60);
    return;
  }

  bool aActive = sensorAActive(); // PIR
  bool bActive = sensorBActive(); // Ultrasonic

  switch (seqState) {
    case IDLE:
      if (aActive) {
        seqState = A_TRIGGERED;
        stateTimestamp = now;
        Serial.println("A triggered (PIR)");
      } else if (bActive) {
        seqState = B_TRIGGERED;
        stateTimestamp = now;
        Serial.println("B triggered (Ultra)");
      }
      break;

    case A_TRIGGERED:
      // waiting for B or timeout
      if (bActive && now - stateTimestamp <= SEQUENCE_WINDOW_MS) {
        handleSequenceAthenB();
      } else if (now - stateTimestamp > SEQUENCE_WINDOW_MS) {
        seqState = IDLE;
        Serial.println("A->? timeout");
      }
      break;

    case B_TRIGGERED:
      if (aActive && now - stateTimestamp <= SEQUENCE_WINDOW_MS) {
        handleSequenceBthenA();
      } else if (now - stateTimestamp > SEQUENCE_WINDOW_MS) {
        seqState = IDLE;
        Serial.println("B->? timeout");
      }
      break;
  }

  // Periodic heartbeat display when idle
  static unsigned long lastHeartbeat = 0;
  if (now - lastHeartbeat > 1000) {
    updateDisplay("Monitoring...");
    lastHeartbeat = now;
  }

  delay(60);
}