from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks.tools import run_task, multitask

hub = PrimeHub()

left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)

left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)
lazer_ranger = UltrasonicSensor(Port.E)

drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=180)

drive_base.settings(600,500,300,200)
drive_base.use_gyro(True)

WALL_DISTANCE = 200  # mm

async def drive_forward():
    """Drive forward continuously using DriveBase."""
    drive_base.drive(400, 0)


async def monitor_distance():
    """Monitor ultrasonic sensor and stop when wall is detected."""
    while True:
        distance = await lazer_ranger.distance()
        print('Distancing...',distance)
        
        if distance < WALL_DISTANCE:
            # Stop the drivebase
            await drive_base.stop
            print(f"Wall detected at {distance}mm!")
            break
        
        # Small delay to prevent overwhelming the sensor
        await wait(50)

async def main():
    await drive_base.straight(-920)
    await drive_base.turn(-90,Stop.HOLD)
    await drive_base.straight(65)
    #Solve
    drive_base.turn(-10)
    await left_arm.run_angle(10000,-4000)
    await drive_base.straight(-110)
    await drive_base.turn(90)

await multitask(
        drive_forward(), 
        monitor_distance()
    )
    
run_task(main())