#define L_TRIG PB0
#define L_ECHO PB1
#define R_TRIG PB10
#define R_ECHO PB11

#define L_LED  PA0
#define R_LED  PA1
#define BUZZER PA2

#define L_SIGNAL PA3
#define R_SIGNAL PA4

const int BLIND_SPOT_CM = 100;   // detection range for "vehicle present"
const int SAMPLE_INTERVAL_MS = 100;

long readDistanceCM(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // 30ms timeout ~ 5m max
  if (duration == 0) return 999; // no echo = nothing in range

  return duration * 0.0343 / 2;  // speed of sound conversion
}

void setup() {
  Serial.begin(115200);

  pinMode(L_TRIG, OUTPUT);
  pinMode(R_TRIG, OUTPUT);
  pinMode(L_ECHO, INPUT);
  pinMode(R_ECHO, INPUT);

  pinMode(L_LED, OUTPUT);
  pinMode(R_LED, OUTPUT);
  pinMode(BUZZER, OUTPUT);

  pinMode(L_SIGNAL, INPUT_PULLUP);
  pinMode(R_SIGNAL, INPUT_PULLUP);

  Serial.println("Blind Spot Detector initialized");
}

void loop() {
  long leftDist  = readDistanceCM(L_TRIG, L_ECHO);
  long rightDist = readDistanceCM(R_TRIG, R_ECHO);

  bool leftOccupied  = (leftDist  <= BLIND_SPOT_CM);
  bool rightOccupied = (rightDist <= BLIND_SPOT_CM);

  bool leftSignalOn  = (digitalRead(L_SIGNAL) == LOW);
  bool rightSignalOn = (digitalRead(R_SIGNAL) == LOW);

  digitalWrite(L_LED, leftOccupied ? HIGH : LOW);
  digitalWrite(R_LED, rightOccupied ? HIGH : LOW);

  bool dangerousMerge = (leftSignalOn && leftOccupied) ||
                         (rightSignalOn && rightOccupied);

  if (dangerousMerge) {
    // rapid beep = active warning
    digitalWrite(BUZZER, HIGH);
    delay(80);
    digitalWrite(BUZZER, LOW);
    delay(80);
  } else {
    digitalWrite(BUZZER, LOW);
  }

  Serial.print("L: "); Serial.print(leftDist); Serial.print(" cm  ");
  Serial.print("R: "); Serial.print(rightDist); Serial.print(" cm  ");
  Serial.print(" | L_sig:"); Serial.print(leftSignalOn);
  Serial.print(" R_sig:"); Serial.println(rightSignalOn);

  delay(SAMPLE_INTERVAL_MS);
}