#include <stdint.h>

#include <avr/interrupt.h>
#include <util/delay.h>
#include <avr/io.h>

#define SN74_SER (1 << PC6)
#define SN74_RCLK (1 << PB5)
#define SN74_SRCLK (1 << PE6)

#define SEG_DIG1 (1 << PF6)
#define SEG_DIG2 (1 << PF5)
#define SEG_DIG3 (1 << PF4)
#define SEG_DIG4 (1 << PF1)

#define HCSR04_TRIG (1 << PB7)
#define HCSR04_ECHO (1 << PB6)

#define SWITCH_IN (1 << PF7)

void init_3461BS() {
    DDRF |= SEG_DIG1 | SEG_DIG2 | SEG_DIG3 | SEG_DIG4;
    PORTF &= ~(SEG_DIG1 | SEG_DIG2 | SEG_DIG3 | SEG_DIG4);
}

void init_74HC595() {
    DDRC |= SN74_SER;
    DDRB |= SN74_RCLK;
    DDRE |= SN74_SRCLK;

    PORTB &= ~SN74_RCLK;
    PORTE &= ~SN74_SRCLK;
}

const uint8_t digit_segments[] = {
    0b10000001, // 0
    0b11001111, // 1
    0b10010010, // 2
    0b10000110, // 3
    0b11001100, // 4
    0b10100100, // 5
    0b10100000, // 6
    0b10001101, // 7
    0b10000000, // 8
    0b10000100, // 9
};

void send_serial_byte(uint8_t n) {
    for (int i = 0; i < 8; i++) {
        if (n & (1 << i)) {
            PORTC |= SN74_SER;
        }
        else {
            PORTC &= ~SN74_SER;
        }
        PORTE |= SN74_SRCLK;
        PORTE &= ~SN74_SRCLK;
        PORTC &= ~SN74_SER;
    }

    PORTB |= SN74_RCLK;
    PORTB &= ~SN74_RCLK;
}

void init_HCSR04() {
    DDRF &= ~(SWITCH_IN);
    PORTF |= SWITCH_IN;

    DDRB |= HCSR04_TRIG;
    DDRB &= ~(HCSR04_ECHO);

    PORTB |= HCSR04_ECHO | HCSR04_TRIG;
}

void setup() {
    init_74HC595();
    init_3461BS();
    init_HCSR04();
}

uint16_t measure_distance() {
    float duration, distance = 0;

    PORTB &= ~HCSR04_TRIG;
    _delay_us(2);
    PORTB |= HCSR04_TRIG;
    _delay_us(10);
    PORTB &= ~HCSR04_TRIG;

    while (!(PINB & HCSR04_ECHO));

    TCNT1 = 0;
    TCCR1B = (1 << CS11);

    while (PINB & HCSR04_ECHO) ;


    uint16_t ticks = TCNT1;
    TCCR1B = 0;

    distance = (((float)ticks / 2) * 0.0343) /2;

    return distance;
}

int main() {
    setup();

    uint16_t measure = 0;

    while (1) {
        if (!(PINF & SWITCH_IN)) {
            measure = measure_distance();
        }
        send_serial_byte(digit_segments[measure / 1000]);
        PORTF |= SEG_DIG1;
        _delay_ms(2);
        PORTF &= ~SEG_DIG1;

        send_serial_byte(digit_segments[(measure / 100) % 10]);
        PORTF |= SEG_DIG2;
        _delay_ms(2);
        PORTF &= ~SEG_DIG2;

        send_serial_byte(digit_segments[(measure / 10) % 10]);
        PORTF |= SEG_DIG3;
        _delay_ms(2);
        PORTF &= ~SEG_DIG3;

        send_serial_byte(digit_segments[measure % 10]);
        PORTF |= SEG_DIG4;
        _delay_ms(2);
        PORTF &= ~SEG_DIG4;
    }
}