from machine import Pin, I2C
from time import sleep_us, sleep
import ssd1306
import time

# Pinovi za HC-SR04
TRIG_PIN = 3  # Trig je povezan na GP3
ECHO_PIN = 2  # Echo je povezan na GP2

# Postavke OLED ekrana
I2C_SCL_PIN = 13  # SCL je povezan na GP5
I2C_SDA_PIN = 12  # SDA je povezan na GP4
OLED_WIDTH = 128
OLED_HEIGHT = 64

# Funkcija za mjerenje udaljenosti
def measure_distance(trig, echo):
    # Početak mjerenja
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()

    # Čekaj početak odjeka
    while echo.value() == 0:
        pass
    start = time.ticks_us()

    # Čekaj kraj odjeka
    while echo.value() == 1:
        pass
    end = time.ticks_us()

    # Izračunaj udaljenost
    duration = time.ticks_diff(end, start)  # Trajanje pulsa u mikrosekundama
    distance = (duration * 0.0343) / 2  # Pretvori u centimetre
    return distance

# Inicijalizacija ultrazvučnog senzora
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

# Inicijalizacija I2C veze i OLED ekrana
i2c = I2C(0, scl=Pin(I2C_SCL_PIN), sda=Pin(I2C_SDA_PIN))
oled = ssd1306.SSD1306_I2C(OLED_WIDTH, OLED_HEIGHT, i2c)

# Glavna petlja
while True:
    try:
        # Mjeri udaljenost
        distance = measure_distance(trig, echo)

        # Očisti ekran
        oled.fill(0)

        # Prikaži tekst na ekranu
        oled.text("Ultrazvucni senzor", 0, 0)
        oled.text("Udaljenost:", 0, 20)
        oled.text("{:.2f} cm".format(distance), 0, 40)

        # Osveži ekran
        oled.show()

        # Sačekaj pre sledećeg merenja
        sleep(1)
    except Exception as e:
        print("Error:", e)
        oled.fill(0)
        oled.text("Error", 0, 0)
        oled.text(str(e), 0, 20)
        oled.show()
        sleep(2)