from machine import I2C, Pin,PWM
import mpu6050
from hcsr04 import HCSR04
from stepper import Stepper
from MyStepper import MyStepper
import time
import oled

# initialize components
stepper = MyStepper(step_pin=14,dir_pin=27)
button = Pin(23, Pin.IN, Pin.PULL_UP)
buzzer = PWM(Pin(19, Pin.OUT),freq = 100,duty=0)
sensor = HCSR04(trigger_pin=25, echo_pin=26)
i2c = I2C(scl=Pin(22), sda=Pin(21))
accel= mpu6050.accel(i2c)
oled = oled.I2C(128, 64, i2c)

# configuration
BRIDGE_DOWN_STEPS = 200
BRIDGE_UP_STEPS = 200
APPROACH_DISTANCE = 30  # cm
DEBOUNCE_MS = 50       # milliseconds
TILT_THRESHOLD = 1.5

def check_tilt():
    vals = accel.get_values()
    return  abs(vals["AcY"]/ 16384) > TILT_THRESHOLD

def update_oled(bridge_state,is_tilted,distance):
    oled.clear()
    tilt_string = 'Tilted' if is_tilted else 'Not Tilted'
    oled.text(f'Bridge is {bridge_state}',1,0)
    oled.text(f'Distance : {distance}',1,1)
    oled.text(tilt_string,1,2)
    oled.show()

def buzz(num_beeps):
    for i in range(num_beeps):
        buzzer.duty(500)
        time.sleep(0.2)
        buzzer.duty(0)
        time.sleep(0.1)

def move_bridge(bridge_state,is_tilted):
    if not is_tilted:
        direction = 'UP'
        if bridge_state == 'UP':
            direction = 'DOWN'
            stepper.move_x_steps(BRIDGE_DOWN_STEPS,1)
        elif bridge_state == 'DOWN':
            direction = 'UP'
            stepper.move_x_steps(BRIDGE_UP_STEPS,0)
        buzz(1)
        return direction
    else:
        buzz(3)
        return bridge_state
        
def main():
    bridge_state = 'UP'
    while True:
        distance = sensor.distance_cm()
        is_tilted = check_tilt()
        if button.value () == 0:
            bridge_state = move_bridge(bridge_state,is_tilted)
        if distance < APPROACH_DISTANCE and not is_tilted and bridge_state == 'UP':
            bridge_state = move_bridge(bridge_state,is_tilted)
            update_oled(bridge_state,is_tilted,distance)
            time.sleep(5)
            bridge_state = move_bridge(bridge_state,is_tilted)

        update_oled(bridge_state,is_tilted,distance)
        time.sleep(0.2)

if __name__ == '__main__':
    main()