#define ledr      PB1
#define ledy      PB5
#define trig      PB0
#define echo      PB7
#define buz       PB4
#define SW2       PB6
#define dataPin   PA5
#define latchPin  PA7
#define clockPin  PA6

const byte SEG_BLANK = 0xFF;
const byte SEG_DASH  = 0b10111111;

const byte pattern[10] = {
  0x81,  
  0xF5,  
  0x49,  
  0x61,  
  0x35,  
  0x23,  
  0x03,  
  0xF1,  
  0x01,  
  0x21   
};

bool lastSwitchState  = HIGH;
int  stableDistance   = -1;   
int  lastPrintedDist  = -1;   

void sendBoth(byte leftByte, byte rightByte) {
  digitalWrite(latchPin, LOW);
  shiftOut(dataPin, clockPin, MSBFIRST, rightByte);
  shiftOut(dataPin, clockPin, MSBFIRST, leftByte);
  digitalWrite(latchPin, HIGH);
}

void showDistance(int cm) {
  if (cm < 0 || cm > 999) { sendBoth(SEG_DASH, SEG_DASH); return; }
  byte L = pattern[cm / 100]        & 0xFE;  
  byte R = pattern[(cm % 100) / 10];         
  sendBoth(L, R);
}

void showDash()  { sendBoth(SEG_DASH,  SEG_DASH);  }
void showBlank() { sendBoth(SEG_BLANK, SEG_BLANK); }

int rawPing() {
  digitalWrite(trig, LOW);
  delayMicroseconds(4);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  long dur = pulseIn(echo, HIGH, 38000UL);
  if (dur == 0) return -1;
  int d = (int)(dur * 0.034 / 2);
  if (d < 2 || d > 400) return -1;
  return d;
}

int stablePing() {
  const int N = 5;
  int readings[N];
  int valid = 0;

  for (int i = 0; i < N; i++) {
    int r = rawPing();
    if (r > 0) readings[valid++] = r;
    delay(62);   
  }

  if (valid < 3) return -1;  

  for (int i = 0; i < valid - 1; i++)
    for (int j = i + 1; j < valid; j++)
      if (readings[j] < readings[i]) {
        int tmp = readings[i];
        readings[i] = readings[j];
        readings[j] = tmp;
      }

  int sum = 0;
  int count = 0;
  for (int i = 1; i < valid - 1; i++) { sum += readings[i]; count++; }
  return (count > 0) ? (sum / count) : -1;
}

void warmUp() {
  for (int i = 0; i < 3; i++) { rawPing(); delay(65); }
}

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

  pinMode(ledr,        OUTPUT);
  pinMode(ledy,        OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(trig,        OUTPUT);
  pinMode(echo,        INPUT);
  pinMode(buz,         OUTPUT);
  pinMode(SW2,         INPUT_PULLUP);
  pinMode(dataPin,     OUTPUT);
  pinMode(latchPin,    OUTPUT);
  pinMode(clockPin,    OUTPUT);

  digitalWrite(ledr, LOW);
  digitalWrite(ledy, LOW);
  digitalWrite(LED_BUILTIN, LOW);
  digitalWrite(trig, LOW);

  sendBoth(pattern[8] & 0xFE, pattern[8]);
  delay(1000);
  showBlank();

}

void loop() {
  bool currentSwitch = digitalRead(SW2);   

  if (lastSwitchState == HIGH && currentSwitch == LOW) {
    showDash();
    noTone(buz);
    delayMicroseconds(200);
    warmUp();
    stableDistance  = -1;
    lastPrintedDist = -1;
  }
  lastSwitchState = currentSwitch;

  if (currentSwitch == HIGH) {
    noTone(buz);
    digitalWrite(ledr,        LOW);
    digitalWrite(ledy,        LOW);
    digitalWrite(LED_BUILTIN, LOW);
    showDistance(0);
    delay(200);
    return;
  }

  noTone(buz);
  delayMicroseconds(200);  

  int measured = stablePing();   

  if (measured > 0) {
    if (abs(measured - stableDistance) > 2) {
      stableDistance = measured;
    }
  } else {
    stableDistance = -1;
  }

  if (stableDistance != lastPrintedDist) {
    if (stableDistance > 0) {
      Serial.print("Distance: ");
      Serial.print(stableDistance);
      Serial.println(" cm");
    } else {
      Serial.println("Distance: out of range");
    }
    lastPrintedDist = stableDistance;
  }

  showDistance(stableDistance);

  if (stableDistance < 0) {
    digitalWrite(ledr,        LOW);
    digitalWrite(ledy,        LOW);
    digitalWrite(LED_BUILTIN, HIGH);

  } else if (stableDistance < 30) {
    digitalWrite(ledr,        HIGH);
    digitalWrite(ledy,        LOW);
    digitalWrite(LED_BUILTIN, LOW);
    tone(buz, 880);
    delay(200);

  } else if (stableDistance < 120) {
    long interval = map(stableDistance, 30, 120, 100, 800);
    digitalWrite(ledr,LOW);
    digitalWrite(ledy, HIGH);
    digitalWrite(LED_BUILTIN, LOW);
    tone(buz, 440);
    delay(80);
    noTone(buz);
    delay(interval);

  } else {
    digitalWrite(ledr,LOW);
    digitalWrite(ledy,LOW);
    digitalWrite(LED_BUILTIN, HIGH);
  }
}