""" LEGO Liebherr LR 13000 / Xbox Controller / Pybricks

    Top hub connects to Xbox controller.
	
	Bottom hub receives the control data from the top one.

    Controls:
    Undercarriage
	    Left track forward ... Left trigger
	    Left track backward ... Left trigger + Button X pushed
	    Right track forward ... Right trigger
	    Right track backward ... Right trigger + Button A pushed
		The superstructure turning ... Right joystick left / right
	Crane
		The boom raising / lowering ... Left joystick up / down
		The jib raising / lowering ... Left joystick left / right
		The hook raising / lowering ... Right joystick up / down		
		The lights on / off ... Button Y
"""

from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor, Light
from pybricks.parameters import Button, Color, Direction, Port
from pybricks.tools import wait
from pybricks.iodevices import XboxController

hub = TechnicHub(observe_channels=[1])
superstr_motor = Motor(Port.B, positive_direction=Direction.COUNTERCLOCKWISE)
left_motor = Motor(Port.C, positive_direction=Direction.CLOCKWISE)
right_motor = Motor(Port.D, positive_direction=Direction.COUNTERCLOCKWISE)

# motors settings
LEFT_TRACK_SPEED = 800
RIGHT_TRACK_SPEED = 800
ROTATESPEED = 240          # motor speed to turn the crane superstructure

while True:

    # Receive broadcast from the other hub.
    data = hub.ble.observe(1)

    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)

        # *data* contains the same values in the same order
        # that were passed to hub.ble.broadcast() on the
        # other hub.
        rot_speed, left_tr_speed, right_tr_speed = data

        # control of the superstructure turning
        superstr_motor.run(rot_speed * ROTATESPEED / 80)

        # left track control
        left_motor.run(left_tr_speed * LEFT_TRACK_SPEED / 100)

        # right track control
        right_motor.run(right_tr_speed * RIGHT_TRACK_SPEED / 100)