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


left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
left_arm = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_arm = Motor(Port.D)

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

drive_base.settings(550,700,100,100)

drive_base.use_gyro(True)

async def Run1():
    left_arm.run_angle(1000, 300)
    right_arm.run_angle(1000,500)
    await drive_base.straight(320)

    await right_arm.run_angle(5000,-500, Stop.HOLD)
    await right_arm.run_angle(5000,500, Stop.HOLD)
    await right_arm.run_angle(5000,-500, Stop.HOLD)
    await right_arm.run_angle(5000,500, Stop.HOLD)
    await right_arm.run_angle(5000,-500, Stop.HOLD)

    await drive_base.turn(-20)
    await drive_base.straight(275)
    await drive_base.turn(20)
    await drive_base.straight(63)

    await drive_base.turn(-30)
    right_arm.run_angle(50,500)
    await drive_base.turn(45)
    await drive_base.straight(-135)
    await drive_base.turn(-60)
    await drive_base.straight(90)
    await left_arm.run_angle(1000,-450)
    await drive_base.straight(-145)
    await left_arm.run_angle(1000,450)
    await drive_base.straight(10)
    await drive_base.turn(35)
    await drive_base.straight(-500)

async def Run2():
    await drive_base.straight(200)
    await drive_base.turn(-20)
    await drive_base.straight(525)
    await drive_base.turn(60)

    await drive_base.straight(50)
    await right_arm.run_angle(2000,1000)
    await drive_base.straight(-50)
    await drive_base.turn(45)
    await drive_base.straight(50)
    await right_arm.run_angle(350,-1000)

    await drive_base.turn(-100)
    await drive_base.straight(-600)


async def Run3():
    await drive_base.straight(915)
    await drive_base.turn(-90)
    await drive_base.straight(60)
    await left_arm.run_angle(10000,-15000)
    await drive_base.straight(-60)
    await drive_base.turn(85)
    await drive_base.straight(2000)


async def Run5():
    await drive_base.straight(420)
    await right_arm.run_angle(300,-100)
    await drive_base.straight(-100)
    await right_arm.run_angle(300, 100)
    await drive_base.straight(-350)


async def Run6():
    await drive_base.straight(420)
    await right_arm.run_angle(300,-100)
    await drive_base.straight(-100)
    await right_arm.run_angle(300, 100)
    await drive_base.straight(-350)


def main():
    if color_sensor.color() == Color.GREEN:
        Run1()
        
    if color_sensor.color() == Color.WHITE:
        Run2()
        
    if color_sensor.color() == Color.YELLOW:
        Run3()
        
    if color_sensor.color() == Color.BLUE:
        Run5()
        
    if color_sensor.color() == Color.RED:
        Run6()
    
    print(f'Detected color: {color_sensor.color()}')

# Main execution loop
while True:
    main()
    wait(100)