#define TRIG_PIN 13
#define ECHO_PIN 12

#define LATCH_PIN 5
#define CLOCK_PIN 4
#define DATA_PIN 3

#define BUZZER_PIN 7
#define LED_PIN 6

// binary representation of digits in single
// 7-segment. Each bit represents the on/off
// status of pins A-G in that order
const int digits_b[10] = {
  0b1111110, 0b0110000, 0b1101101, 0b1111001, 0b0110011,
  0b1011011, 0b1011111, 0b1110000, 0b1111111, 0b01111011 
};

long long last_flash_millis = 0;
bool last_flash_state = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT_PULLUP);

  pinMode(LATCH_PIN, OUTPUT);
  pinMode(CLOCK_PIN, OUTPUT);
  pinMode(DATA_PIN, OUTPUT);
  
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
}

void write_digit(int digit) {
  byte binary = digits_b[digit];

  for (int i = 0; i < 7; ++i) {
      bool bitstate = binary & (1 << i);
      digitalWrite(CLOCK_PIN, LOW);
      digitalWrite(DATA_PIN, !bitstate);
      digitalWrite(CLOCK_PIN, HIGH);
  }
}

void flash_digit_pin(int pos) {
  // flash the digits invididually and sequentially with 
  // low interval so as to trick the eye 

  // i < 7 because bits are pushed downwards Q1 ... Q7,
  // as new bits are added, NOT placed directly Q7 ... Q1
  for (int i = 0; i < 8; ++i) {
      bool bitstate = (i == pos);
      digitalWrite(CLOCK_PIN, LOW);
      digitalWrite(DATA_PIN, bitstate);
      digitalWrite(CLOCK_PIN, HIGH);
  }
}

void display_segment(int number) {
  const int digits[4] = {
    (number % 10000) / 1000,
    (number % 1000) / 100,
    (number % 100) / 10,
    (number % 10) / 1
  }; // 4 digits at most
  for (int pos = 0; pos < 4; ++pos) {
    digitalWrite(LATCH_PIN, LOW);
    write_digit(digits[pos]);
    flash_digit_pin(pos);
    digitalWrite(LATCH_PIN, HIGH);
    delay(20);
  }
}

int read_ultrasonic_distance() {
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(1000);
  digitalWrite(TRIG_PIN, LOW);
  return pulseIn(ECHO_PIN, HIGH);
}

void loop() {
  // put your main code here, to run repeatedly:
  // time to return in microseconds
  int time_to_return = read_ultrasonic_distance();
  int distance_cm = time_to_return / 2 * 0.034; // 343 m/s => 0.000343m/microsecond
  
  if (distance_cm < 100) {
    display_segment(distance_cm);

    if (millis() - last_flash_millis >= 200) {
      last_flash_millis = millis();
      if (last_flash_state) {
        tone(BUZZER_PIN, 440);
        digitalWrite(LED_PIN, HIGH);
      } else {
        noTone(BUZZER_PIN);
        digitalWrite(LED_PIN, LOW);
      }
      last_flash_state = !last_flash_state;
    }
  }
  else {
    display_segment(9999);
  }

  delay(20);
}