'''
TRIG = 16
ECHO = 17
'''
from machine import Pin, time_pulse_us
import time
from utime import sleep_us

class Port:
    def __init__(self, pin_nums):
        self.pins = [Pin(num, Pin.OUT) for num in pin_nums]

    def write(self, value):
        for i, p in enumerate(self.pins):
            p.value((value >> i) & 1)

echo = Pin(17,Pin.IN)

def read_sensor():
    trig = Pin(16,Pin.OUT)
    trig.value(0)
    sleep_us(5)
    trig.value(1)
    sleep_us(10)
    trig.value(0)


    pulse_time = time_pulse_us(echo, 1, 500*2*30)
    cm = pulse_time*0.017   # 1cm each 29.1us
    return cm

def LCD_cmd(RS,E,LCD_Port,cmd):
    RS.value(0)
    MSN = (cmd & 0xF0) >> 4
    LSN = cmd & 0x0F
    LCD_Port.write(MSN)
    E.value(1)
    time.sleep(0.005)
    E.value(0)
    time.sleep(0.005)
    LCD_Port.write(LSN)
    E.value(1)
    time.sleep(0.005)
    E.value(0)
    time.sleep(0.005)

def LCD_data(RS,E,LCD_Port,data):
    RS.value(1)
    MSN = (data & 0xF0) >> 4
    LSN = data & 0x0F
    LCD_Port.write(MSN)
    E.value(1)
    time.sleep(0.005)
    E.value(0)
    time.sleep(0.005)
    LCD_Port.write(LSN)
    E.value(1)
    time.sleep(0.005)
    E.value(0)
    time.sleep(0.005)

def LCD_init(RS,E,LCD_Port):
    LCD_cmd(RS,E,LCD_Port,0x32)
    LCD_cmd(RS,E,LCD_Port,0x0C)
    LCD_cmd(RS,E,LCD_Port,0x01)
    time.sleep(0.002)               # זמן מחיקה ארוך יותר
    LCD_cmd(RS,E,LCD_Port,0x06)

def LCD_Print(RS,E,LCD_Port,string):
    for val in string:
        LCD_data(RS,E,LCD_Port,ord(val))


def LCD_setCursor(RS,E,LCD_Port,x,y):
    if(y==1):
        LCD_cmd(RS,E,LCD_Port,0x80 | x)

    elif(y==2):
        LCD_cmd(RS,E,LCD_Port,0xC0 | x)


LCD_Port = Port([16, 4, 0, 2])  # D4, D5, D6, D7

RS = Pin(23,Pin.OUT, value=0)
E = Pin(22,Pin.OUT, value=0)

LCD_init(RS,E,LCD_Port)
while True:
    d = read_sensor()
    LCD_setCursor(RS,E,LCD_Port,0,1)
    LCD_Print(RS,E,LCD_Port,f'Distance [cm]:')
    LCD_setCursor(RS,E,LCD_Port,0,2)
    LCD_Print(RS,E,LCD_Port,f'{d:.2f}   ')
    print(f'Distance = {d:.2f}[cm]')
    time.sleep(1)