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

hub = PrimeHub(observe_channels=[2])

right_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
left_motor = Motor(Port.E)
hub.speaker.volume(100)
color_sensor = ColorSensor(Port.C)

drive_base = DriveBase(left_motor, right_motor, wheel_diameter=56, axle_track=192)

while True:
    # Receive broadcast from the other hub.

    data = hub.ble.observe(2)    
    if data is None:
        # No data has been received in the last 1 second.
        hub.light.on(Color.RED)
    else:
        
        # Data was received and is less that one second old.
        hub.light.on(Color.GREEN)
        bl, br, mba, mda, tap, tcp, tep = data 
        #bl, br, mba, mda, tap, tcp, tep = data 

        #if name == 2:
        #    continue
        if tep:
            hub.speaker.beep(1800, 80)
        if (mba > 270):
            mba = 270
        if (mba < 90):
            mba = 90        

        if (mda > 180):
            mda = 180
        if (mda < 10):
            drive_base.stop()
        else:
            if not tap:
                if not tcp:
                    mda = 0
                    drive_base.stop()
                else:
                    mda = -mda
            drive_base.drive(mda, (180 - mba) / 2)
          
        #print("L:" + str(bl) + ", R: " + str(br) + ", mb: " + str(mba) + ",md: " + str(mda) + ",TA: " + str(tap) + ", TC: " + str(tcp) + ", TE: " + str(tep))
        for x in range(1, 5):
            wait(10)
            if color_sensor.color(True) == Color.WHITE:
                drive_base.drive(-mda, 0)
                wait(1000)