/*
 * Smart Parking Guidance System
 * Board: STM32 Nucleo-F446RE
 * Sensors: HC-SR04 Ultrasonic Sensors (up to 4 parking spots)
 * Features: Real-time parking spot detection, LED indicators, Serial output
 * 
 * Wiring:
 * Spot 1: TRIG=D2, ECHO=D3, LED=D4
 * Spot 2: TRIG=D5, ECHO=D6, LED=D7
 * Spot 3: TRIG=D8, ECHO=D9, LED=D10
 * Spot 4: TRIG=D11, ECHO=D12, LED=D13
 */

// Pin definitions for 4 parking spots
#define NUM_SPOTS 4

// Ultrasonic sensor pins
const int TRIG_PINS[NUM_SPOTS] = {D2, D5, D8, D11};
const int ECHO_PINS[NUM_SPOTS] = {D3, D6, D9, D12};

// LED indicator pins (Green = Available, Red = Occupied)
const int LED_PINS[NUM_SPOTS] = {D4, D7, D10, LED_BUILTIN};

// Parking configuration
#define DISTANCE_THRESHOLD 30  // Distance in cm (below = occupied)
#define SAMPLE_DELAY 100       // Delay between sensor readings (ms)
#define DEBOUNCE_COUNT 3       // Number of consistent readings needed

// Parking spot structure
struct ParkingSpot {
  bool occupied;
  int debounceCounter;
  long lastDistance;
};

ParkingSpot spots[NUM_SPOTS];
int availableSpots = NUM_SPOTS;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);
  while (!Serial && millis() < 3000); // Wait up to 3 seconds for serial
  
  Serial.println("=================================");
  Serial.println("Smart Parking Guidance System");
  Serial.println("STM32 Nucleo-F446RE");
  Serial.println("=================================");
  
  // Initialize pins
  for (int i = 0; i < NUM_SPOTS; i++) {
    pinMode(TRIG_PINS[i], OUTPUT);
    pinMode(ECHO_PINS[i], INPUT);
    pinMode(LED_PINS[i], OUTPUT);
    
    // Initialize spot status
    spots[i].occupied = false;
    spots[i].debounceCounter = 0;
    spots[i].lastDistance = 0;
    
    // Turn on all LEDs (available state)
    digitalWrite(LED_PINS[i], HIGH);
  }
  
  Serial.println("\nSystem initialized!");
  Serial.println("Monitoring parking spots...\n");
  delay(1000);
}

void loop() {
  bool statusChanged = false;
  
  // Check each parking spot
  for (int i = 0; i < NUM_SPOTS; i++) {
    long distance = measureDistance(i);
    
    // Update spot status with debouncing
    if (updateSpotStatus(i, distance)) {
      statusChanged = true;
    }
    
    // Update LED indicator
    updateLED(i);
  }
  
  // Print status if changed
  if (statusChanged) {
    printParkingStatus();
  }
  
  delay(SAMPLE_DELAY);
}

// Measure distance using ultrasonic sensor
long measureDistance(int spotIndex) {
  // Send ultrasonic pulse
  digitalWrite(TRIG_PINS[spotIndex], LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PINS[spotIndex], HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PINS[spotIndex], LOW);
  
  // Read echo pulse
  long duration = pulseIn(ECHO_PINS[spotIndex], HIGH, 30000); // 30ms timeout
  
  // Calculate distance in cm (speed of sound = 343 m/s)
  long distance = duration * 0.0343 / 2;
  
  // Handle timeout (no object detected)
  if (duration == 0) {
    distance = 400; // Max range
  }
  
  return distance;
}

// Update spot status with debouncing
bool updateSpotStatus(int spotIndex, long distance) {
  bool newState = (distance < DISTANCE_THRESHOLD);
  bool changed = false;
  
  // Check if state is consistent
  if (newState != spots[spotIndex].occupied) {
    spots[spotIndex].debounceCounter++;
    
    // Confirm state change after consistent readings
    if (spots[spotIndex].debounceCounter >= DEBOUNCE_COUNT) {
      spots[spotIndex].occupied = newState;
      spots[spotIndex].debounceCounter = 0;
      changed = true;
      
      // Update available spots counter
      if (newState) {
        availableSpots--;
      } else {
        availableSpots++;
      }
    }
  } else {
    spots[spotIndex].debounceCounter = 0;
  }
  
  spots[spotIndex].lastDistance = distance;
  return changed;
}

// Update LED indicator
void updateLED(int spotIndex) {
  if (spots[spotIndex].occupied) {
    // Red (occupied) - blink pattern
    digitalWrite(LED_PINS[spotIndex], (millis() / 500) % 2);
  } else {
    // Green (available) - solid on
    digitalWrite(LED_PINS[spotIndex], HIGH);
  }
}

// Print parking status to serial
void printParkingStatus() {
  Serial.println("\n========== PARKING STATUS ==========");
  Serial.print("Available Spots: ");
  Serial.print(availableSpots);
  Serial.print(" / ");
  Serial.println(NUM_SPOTS);
  Serial.println("------------------------------------");
  
  for (int i = 0; i < NUM_SPOTS; i++) {
    Serial.print("Spot ");
    Serial.print(i + 1);
    Serial.print(": ");
    
    if (spots[i].occupied) {
      Serial.print("[OCCUPIED] ");
    } else {
      Serial.print("[AVAILABLE]");
    }
    
    Serial.print(" Distance: ");
    Serial.print(spots[i].lastDistance);
    Serial.println(" cm");
  }
  
  Serial.println("====================================\n");
}