// ===============================
// PHASE 1 - Parking Sensor System
// Distance measurement + UART + LEDs control
// ===============================

// Define pins for HC-SR04
#define TRIG_PIN D5
#define ECHO_PIN D6

// Define LED pins
#define LED_GREEN D2
#define LED_ORANGE D4
#define LED_RED D3

// Variable to store measured time and distance
long duration;
float distance;

// -------------------------------
// Function to measure distance
// -------------------------------
float readDistance() {
  
  // Send trigger pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read echo time
  duration = pulseIn(ECHO_PIN, HIGH);

  // Convert time to distance (cm)
  distance = duration * 0.034 / 2;

  return distance;
}

// -------------------------------
// Setup function (runs once)
// -------------------------------
void setup() {
  
  // Initialize serial communication (UART via VCOM)
  Serial.begin(9600);

  // Configure pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_ORANGE, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  Serial.println("System initialized - Phase 1");
}

// -------------------------------
// Main loop (runs continuously)
// -------------------------------
void loop() {

  // Read distance from sensor
  float d = readDistance();

  // Display distance on UART terminal
  Serial.print("Distance: ");
  Serial.print(d);
  Serial.println(" cm");

  // ---------------------------
  // LED control based on distance
  // ---------------------------
  
  if (d > 120) {
    // Safe distance
    digitalWrite(LED_GREEN, HIGH);
    digitalWrite(LED_BUILTIN, HIGH);
    digitalWrite(LED_ORANGE, LOW);
    digitalWrite(LED_RED, LOW);
  }
  else if (d >= 30) {
    // Medium distance
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_BUILTIN, LOW);
    digitalWrite(LED_ORANGE, HIGH);
    digitalWrite(LED_RED, LOW);
  }
  else {
    // Danger close obstacle
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_BUILTIN, LOW);
    digitalWrite(LED_ORANGE, LOW);
    digitalWrite(LED_RED, HIGH);
  }

  delay(100); // small delay for stability
}