#define r1_trigA 5
#define r1_echoA 18
#define r1_trigB 17
#define r1_echoB 16

#define r2_trigA 19
#define r2_echoA 21
#define r2_trigB 27
#define r2_echoB 26

// Road 1 lights
#define r1_red 13
#define r1_yellow 12
#define r1_green 14

// Road 2 lights
#define r2_red 22
#define r2_yellow 2
#define r2_green 23

// Flash lights
#define flash1 4
#define flash2 15

long r1A, r1B, r2A, r2B;

// ✅ Distance function with filter
long getDistance(int trig, int echo) {
  long duration;
  long distance;

  digitalWrite(trig, LOW);
  delayMicroseconds(2);

  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  duration = pulseIn(echo, HIGH, 30000);

  if (duration == 0) return 400; // no object

  distance = duration * 0.034 / 2;

  // 🔴 Ignore 100 cm (no object case)
  if (distance >= 100) return 400;

  return distance;
}

void setup() {
  pinMode(r1_trigA, OUTPUT);
  pinMode(r1_echoA, INPUT);
  pinMode(r1_trigB, OUTPUT);
  pinMode(r1_echoB, INPUT);

  pinMode(r2_trigA, OUTPUT);
  pinMode(r2_echoA, INPUT);
  pinMode(r2_trigB, OUTPUT);
  pinMode(r2_echoB, INPUT);

  pinMode(r1_red, OUTPUT);
  pinMode(r1_yellow, OUTPUT);
  pinMode(r1_green, OUTPUT);

  pinMode(r2_red, OUTPUT);
  pinMode(r2_yellow, OUTPUT);
  pinMode(r2_green, OUTPUT);

  pinMode(flash1, OUTPUT);
  pinMode(flash2, OUTPUT);

  Serial.begin(115200);
}

void loop() {

  r1A = getDistance(r1_trigA, r1_echoA);
  r1B = getDistance(r1_trigB, r1_echoB);
  r2A = getDistance(r2_trigA, r2_echoA);
  r2B = getDistance(r2_trigB, r2_echoB);

  Serial.print("R1A: "); Serial.print(r1A);
  Serial.print(" R1B: "); Serial.print(r1B);
  Serial.print(" R2A: "); Serial.print(r2A);
  Serial.print(" R2B: "); Serial.println(r2B);

  // 🚦 BOTH ARRIVE → YELLOW BLINK
  if (r1A < 20 && r2A < 20) {
    digitalWrite(r1_green, LOW);
    digitalWrite(r2_green, LOW);
    digitalWrite(r1_red, LOW);
    digitalWrite(r2_red, LOW);

    digitalWrite(r1_yellow, HIGH);
    digitalWrite(r2_yellow, HIGH);
    delay(300);
    digitalWrite(r1_yellow, LOW);
    digitalWrite(r2_yellow, LOW);
    delay(300);

    digitalWrite(flash1, LOW);
    digitalWrite(flash2, LOW);
  }

  // 🚦 ROAD 1 PRIORITY
  else if (r1A < 20) {
    digitalWrite(r1_green, HIGH);
    digitalWrite(r1_red, LOW);
    digitalWrite(r1_yellow, LOW);

    digitalWrite(r2_red, HIGH);
    digitalWrite(r2_green, LOW);
    digitalWrite(r2_yellow, LOW);

    // 🚨 Road 2 violation
    if (r2B < 15) {
      digitalWrite(flash2, HIGH);
    } else {
      digitalWrite(flash2, LOW);
    }

    digitalWrite(flash1, LOW);
  }

  // 🚦 ROAD 2 PRIORITY
  else if (r2A < 20) {
    digitalWrite(r2_green, HIGH);
    digitalWrite(r2_red, LOW);
    digitalWrite(r2_yellow, LOW);

    digitalWrite(r1_red, HIGH);
    digitalWrite(r1_green, LOW);
    digitalWrite(r1_yellow, LOW);

    // 🚨 Road 1 violation
    if (r1B < 15) {
      digitalWrite(flash1, HIGH);
    } else {
      digitalWrite(flash1, LOW);
    }

    digitalWrite(flash2, LOW);
  }

  // 🚦 NO VEHICLE
  else {
    digitalWrite(r1_red, LOW);
    digitalWrite(r1_yellow, LOW);
    digitalWrite(r1_green, LOW);

    digitalWrite(r2_red, LOW);
    digitalWrite(r2_yellow, LOW);
    digitalWrite(r2_green, LOW);

    digitalWrite(flash1, LOW);
    digitalWrite(flash2, LOW);
  }

  delay(200);
}