#define PIR_PIN         4
#define TRIG_PIN        3
#define ECHO_PIN        2
#define RELAY_PIN       8

// --- CONFIGURATION CONSTANTS ---
const int FLOOR_DISTANCE_CM = 200;    // Height from sensor to floor (or max limit for testing)
const int TODDLER_HEIGHT_CM = 100;    // Height threshold for toddler detection
const int MIN_VALID_HEIGHT = 20;      // Minimum height to be considered an object (ignore noise/pets)
const unsigned long SEQUENCE_TIMEOUT = 3000; // 3 seconds to complete the walk-through sequence

// --- VARIABLES ---
int personCount = 0;
long duration;
int distance;
bool pirState = LOW;
bool relayState = LOW;

// --- STATE MACHINE FOR DIRECTION ---
enum SequenceState {
  IDLE,
  WAITING_FOR_US,  // PIR triggered first (Entry?)
  WAITING_FOR_PIR  // US triggered first (Exit?)
};

SequenceState currentState = IDLE;
unsigned long stateStartTime = 0;

void setup() {
  Serial.begin(9600);
  
  pinMode(PIR_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RELAY_PIN, OUTPUT);
  
  // Initialize Relay (Assuming Active HIGH relay, change to HIGH if Active LOW)
  digitalWrite(RELAY_PIN, LOW);
  
  Serial.println("--- System Initialized ---");
  Serial.print("Floor Calibration: "); Serial.print(FLOOR_DISTANCE_CM); Serial.println(" cm");
}

void loop() {
  // 1. Read Sensors
  int currentDistance = getUltrasonicDistance();
  int currentPir = digitalRead(PIR_PIN);
  
  // Calculate Object Height
  int objectHeight = FLOOR_DISTANCE_CM - currentDistance;
  
  // 2. The Limiter: Ignore detections beyond the floor distance
  bool objectDetectedByUS = (currentDistance < (FLOOR_DISTANCE_CM - 10) && currentDistance > 0); 

  // 3. Toddler Detection Logic (Reporting Only)
  if (objectDetectedByUS) {
    if (objectHeight > MIN_VALID_HEIGHT && objectHeight <= TODDLER_HEIGHT_CM) {
      Serial.print("ALERT: Possible Unsupervised Toddler! Height: ");
      Serial.print(objectHeight);
      Serial.println(" cm");
    }
  }

  // 4. Directional Counting State Machine
  unsigned long now = millis();

  switch (currentState) {
    
    // --- IDLE STATE ---
    case IDLE:
      if (currentPir == HIGH && !objectDetectedByUS) {
        // PIR Triggered First -> Potential Entry
        currentState = WAITING_FOR_US;
        stateStartTime = now;
        Serial.println("Sequence Started: PIR First (Expecting Entry)");
      } 
      else if (objectDetectedByUS && currentPir == LOW) {
        // US Triggered First -> Potential Exit
        currentState = WAITING_FOR_PIR;
        stateStartTime = now;
        Serial.println("Sequence Started: US First (Expecting Exit)");
      }
      break;

    // --- WAITING FOR ULTRASONIC (ENTRY) ---
    case WAITING_FOR_US:
      if (objectDetectedByUS) {
        personCount++;
        Serial.println("Action: Person ENTERED");
        resetState();
        delay(1000); // Debounce to prevent double counting
      }
      // Timeout check
      else if (now - stateStartTime > SEQUENCE_TIMEOUT) {
        Serial.println("Timeout: Entry sequence failed");
        resetState();
      }
      break;

    // --- WAITING FOR PIR (EXIT) ---
    case WAITING_FOR_PIR:
      if (currentPir == HIGH) {
        if (personCount > 0) {
          personCount--;
          Serial.println("Action: Person EXITED");
        } else {
          Serial.println("Action: Exit detected but count is 0 (Correction)");
        }
        resetState();
        delay(1000); // Debounce
      }
      // Timeout check
      else if (now - stateStartTime > SEQUENCE_TIMEOUT) {
        Serial.println("Timeout: Exit sequence failed");
        resetState();
      }
      break;
  }

  // 5. Relay Control Logic
  updateRelay();
  
  // Small delay for stability
  delay(100);
}

void resetState() {
  currentState = IDLE;
  Serial.print("Current Occupancy: ");
  Serial.println(personCount);
}

void updateRelay() {
  if (personCount > 0) {
    digitalWrite(RELAY_PIN, HIGH); // Turn ON light
  } else {
    digitalWrite(RELAY_PIN, LOW);  // Turn OFF light
  }
}

int getUltrasonicDistance() {
  // Clear trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  
  // Send 10us pulse
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  // Read echo
  duration = pulseIn(ECHO_PIN, HIGH, 25000); // Timeout 25ms (approx 4m range)
  
  if (duration == 0) {
    return FLOOR_DISTANCE_CM + 10; // Return "No Object" value if timeout
  }
  
  // Calculate distance in cm
  distance = duration * 0.034 / 2;
  return distance;
}