from machine import Pin, I2C
from ssd1306 import SSD1306_I2C
import framebuf, sys
import utime

utime.sleep(0.1) # Wait for USB to become ready
print("Povam!")

direction = Pin(0, Pin.OUT)
step = Pin(1, Pin.OUT)
step.value(0)

trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

pix_res_x = 128
pix_res_y = 64

def init_i2c(scl_pin, sda_pin):
    # Initialize I2C device
    i2c_dev = I2C(1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=200000)
    i2c_addr = [hex(ii) for ii in i2c_dev.scan()]
    
    if not i2c_addr:
        print('No I2C Display Found')
        sys.exit()
    else:
        print("I2C Address      : {}".format(i2c_addr[0]))
        print("I2C Configuration: {}".format(i2c_dev))
    
    return i2c_dev

def move_motor(steps, clockwise, delay):
    direction.value(clockwise)  # Set direction: HIGH for CW, LOW for CCW
    for d in range(steps):
        step.value(1)  
        utime.sleep(delay)  
        step.value(0)  
        utime.sleep(delay)  

def ultra():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(5)
    trigger.low()
    while echo.value() == 0:
        signaloff = utime.ticks_us()
    while echo.value() == 1:
        signalon = utime.ticks_us()
    timepassed = signalon - signaloff
    distance = (timepassed * 0.0343) / 2
    print("The distance from object is ",distance,"cm")
    if (130<distance<170):
        print("This is an ideal distance!")
        oled.fill_rect(5, 20, oled.width - 5, 8, 0)
        oled.text("ideal distance", 5, 20)
        oled.fill_rect(5, 30, oled.width - 5, 8, 0)
        oled.text("motor turning..", 5, 30)
        oled.fill_rect(5, 40, oled.width - 5, 8, 0)
        oled.text("you win!", 5, 40)
        oled.show()
        move_motor(200,1,0.005)
        utime.sleep(0.5) 
        move_motor(200,0,0.005)
        utime.sleep(0.5) 
    else:  
        oled.fill_rect(5, 20, oled.width - 5, 8, 0)
        oled.text("not right", 5, 20)
        oled.fill_rect(5, 30, oled.width - 5, 8, 0)
        oled.text("distance:", 5, 30)
        oled.text(str(distance) + " cm", 5, 40)
        oled.fill_rect(5, 40, oled.width - 5, 8, 0)
        oled.text("you lose!", 5, 40)
        oled.show()

i2c_dev = init_i2c(scl_pin=7, sda_pin=6)
oled = SSD1306_I2C(pix_res_x, pix_res_y, i2c_dev)

while True:
    ultra()
    utime.sleep(0.5)