#include "millis.h"
#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>

#define DATA 0
#define CLOCK 1
#define LATCH 2

int awaitingEcho;
unsigned long start;
unsigned long duration;
unsigned long interval;

const int DIGITS [10] = {
    0b00111111, 0b00000110, 0b01011011, 0b01001111, 0b01100110,
    0b01101101, 0b01111101, 0b00000111, 0b01111111, 0b01101111
};

ISR(INT1_vect) 
{
  if (PIND >> 3 & 1)
  {
    start = millis() * 1000;
  }
  else
  {
    duration = (millis() * 1000) - start;
    awaitingEcho = 0;
  }
}

void show_distance(int distance)
{
    int index_1 = distance / 100;
    int index_2 = (distance / 10) % 10;
    int index_3 = distance % 10;

    int digits[3] = { 
      DIGITS[index_1],
      DIGITS[index_2],
      DIGITS[index_3]
    };
    
    digits[0] = index_1 == 0 ? 0 : digits[0];
    digits[1] = index_1 == 0 && index_2 == 0 ? 0 : digits[1];

    for (int i = 2; i >= 0; --i)
    {
      int j = 7;
      for (int k = 0; k < 8; ++k)
      {
          if ((digits[i] >> j) & 1)
          {
            PORTB &= ~(1 << DATA);
          }
          else
          {
            PORTB |= 1 << DATA;
          }

          _delay_us(10);
          PORTB |= 1 << CLOCK;
          _delay_us(10);
          PORTB &= ~(1 << CLOCK);
          _delay_us(10);
          j -= 1;
      }
    }

    _delay_us(10);
    PORTB |= 1 << LATCH;
    _delay_us(10);
    PORTB &= ~(1 << LATCH);
    _delay_us(10);
}

int main()
{
  EICRA |= 1 << 2;
  EIMSK |= 1 << 1;
  DDRD |= 1 << 4;
  DDRB |= 1 << DATA;
  DDRB |= 1 << CLOCK;
  DDRB |= 1 << LATCH;
  init_millis(16000000UL);
  sei();

  while(1)
  {
    unsigned long now = millis();
    if (now - interval > 100 && !awaitingEcho)
    {
      show_distance(duration * 0.034 / 2.0);
      PORTD |= 1 << 4;
      _delay_us(10);
      PORTD &= ~(1 << 4);
      awaitingEcho = 1;
      interval = now;
    }
  }
}