#include <NewPing.h>

#define TRIG_PIN_LEFT 2
#define ECHO_PIN_LEFT 3
#define TRIG_PIN_RIGHT 4
#define ECHO_PIN_RIGHT 5
#define GREEN_LED_PIN 6
#define YELLOW_LED_PIN 7
#define LEFT_CAR_BUTTON 8
#define RIGHT_CAR_BUTTON 9

const int MAX_DISTANCE = 200; // Maximum distance we want to ping for (in centimeters).
const int DISTANCE_THRESHOLD = 100; // Distance in cm to trigger detection
const int DELAY_TIME = 1000; // Delay between readings in milliseconds

NewPing sonarLeft(TRIG_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE);
NewPing sonarRight(TRIG_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE);

int leftVirtualCarDistance = MAX_DISTANCE;
int rightVirtualCarDistance = MAX_DISTANCE;

void setup() {
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  pinMode(LEFT_CAR_BUTTON, INPUT_PULLUP);
  pinMode(RIGHT_CAR_BUTTON, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop() {
  // Read real sensor data
  int leftRealDistance = sonarLeft.ping_cm();
  int rightRealDistance = sonarRight.ping_cm();
  
  // Handle virtual car movement
  if (digitalRead(LEFT_CAR_BUTTON) == LOW) {
    leftVirtualCarDistance = max(0, leftVirtualCarDistance - 10);
  } else {
    leftVirtualCarDistance = min(MAX_DISTANCE, leftVirtualCarDistance + 5);
  }
  
  if (digitalRead(RIGHT_CAR_BUTTON) == LOW) {
    rightVirtualCarDistance = max(0, rightVirtualCarDistance - 10);
  } else {
    rightVirtualCarDistance = min(MAX_DISTANCE, rightVirtualCarDistance + 5);
  }
  
  // Determine the closest object on each side (real or virtual)
  int leftDistance = min(leftRealDistance == 0 ? MAX_DISTANCE : leftRealDistance, leftVirtualCarDistance);
  int rightDistance = min(rightRealDistance == 0 ? MAX_DISTANCE : rightRealDistance, rightVirtualCarDistance);
  
  // Determine if cars are detected
  bool leftDetected = leftDistance < DISTANCE_THRESHOLD && leftDistance > 0;
  bool rightDetected = rightDistance < DISTANCE_THRESHOLD && rightDistance > 0;
  
  // Control LEDs based on detection
  if (leftDetected && rightDetected) {
    digitalWrite(GREEN_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, HIGH);
    Serial.println("Yellow: Cars detected on both sides");
  } else if (leftDetected || rightDetected) {
    digitalWrite(GREEN_LED_PIN, HIGH);
    digitalWrite(YELLOW_LED_PIN, LOW);
    Serial.println("Green: Car detected on one side");
  } else {
    digitalWrite(GREEN_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, LOW);
    Serial.println("Off: No cars detected");
  }
  
  // Print distances for debugging
  Serial.print("Left real: ");
  Serial.print(leftRealDistance);
  Serial.print(" cm, Left virtual: ");
  Serial.print(leftVirtualCarDistance);
  Serial.print(" cm, Right real: ");
  Serial.print(rightRealDistance);
  Serial.print(" cm, Right virtual: ");
  Serial.print(rightVirtualCarDistance);
  Serial.println(" cm");
  
  delay(DELAY_TIME);
}