import machine
import utime
from ST7735 import TFT
from sysfont import sysfont

# Postavke HC-SR04 senzora
TRIG_PIN = 17  # GPIO za TRIG
ECHO_PIN = 16  # GPIO za ECHO
trig = machine.Pin(TRIG_PIN, machine.Pin.OUT)
echo = machine.Pin(ECHO_PIN, machine.Pin.IN)

# Postavke TFT ekrana
spi = machine.SPI(1, baudrate=20000000, polarity=0, phase=0,
                  sck=machine.Pin(10), mosi=machine.Pin(11), miso=None)
tft = TFT(spi, 14, 15, 13)
tft.initg()
tft.rgb(True)

# Funkcija za mjerenje udaljenosti
def measure_distance():
    trig.value(0)
    utime.sleep_us(2)
    trig.value(1)
    utime.sleep_us(10)
    trig.value(0)
    
    while echo.value() == 0:
        signal_off = utime.ticks_us()
    while echo.value() == 1:
        signal_on = utime.ticks_us()
    
    time_passed = utime.ticks_diff(signal_on, signal_off)
    distance = (time_passed * 0.0343) / 2  # Udaljenost u cm
    return distance

# Funkcija za ažuriranje TFT ekrana
def update_display(distance):
    tft.fill(TFT.WHITE)  # Očisti ekran
    tft.text((10, 10), "Udaljenost:", TFT.BLACK, sysfont, 1.5)
    tft.text((10, 30), "{:.2f} cm".format(distance), TFT.BLUE, sysfont, 1.5)  # Udaljenost u drugom redu

# Glavna petlja
while True:
    try:
        distance = measure_distance()
        print("Udaljenost: {:.2f} cm".format(distance))
        
        update_display(distance)  # Ažuriraj prikaz na displeju
        
        utime.sleep(0.5)  # Kratka pauza
    except Exception as e:
        print("Greška:", e)