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(broadcast_channel=2)

print("ahoj")

hub.light.on(Color.GREEN)
wait(100)
hub.light.on(Color.RED)
wait(50)

print("joha")

hub.light.on(Color.GREEN)
wait(100)
hub.light.on(Color.RED)
wait(50)

print("cau")

hub.light.on(Color.GREEN)
wait(100)
hub.light.on(Color.RED)
wait(50)

print("uac")

mB = Motor(Port.B)
mD = Motor(Port.D)
TA = ForceSensor(Port.A)
TC = ForceSensor(Port.C)
TE = ForceSensor(Port.E)


while True:
  pressed = hub.buttons.pressed()
  #print("L:" + str(Button.LEFT in pressed) + ", R: " + str(Button.RIGHT in pressed) + ", mb: " + str(mB.angle() % 360) + ",md: " + str(mD.angle() % 360) + ",TA: " + str(TA.pressed()) + ", TC: " + str(TC.pressed()) + ", TE: " + str(TE.pressed()))
#  data = (1, Button.LEFT in pressed, Button.RIGHT in pressed, (mB.angle() % 360), (mD.angle() % 360), TA.pressed(), TC.pressed(), TE.pressed())
  data = (Button.LEFT in pressed, Button.RIGHT in pressed, (mB.angle() % 360), (mD.angle() % 360), TA.pressed(), TC.pressed(), TE.pressed())
  hub.ble.broadcast(data)
  wait(20)