""" 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(broadcast_channel=1)
controller = XboxController()
boom_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
hook_motor = Motor(Port.B, positive_direction=Direction.COUNTERCLOCKWISE)
jib_motor = Motor(Port.D, positive_direction=Direction.CLOCKWISE)
light = Light(Port.C)

# motors settings
BOOMSPEED = 700
JIBSPEED = 700
HOOKSPEED = 800

light.off()
light_on = False

hub.light.on(Color.GREEN)

while True:

    # Lights on/off
    now_pressed = controller.buttons.pressed()
    if "Y" in now_pressed:
        if light_on:
            light.off()
            light_on = False
            wait(10)
        else:
            light.on(100)
            light_on = True
            wait(10)
        while "Y" in controller.buttons.pressed():  # wait until released
            wait(10)

    # control of the boom raising and lowering
    boom_speed = 0
    horizontal, vertical = controller.joystick_left()
    if vertical < -20:
        boom_speed = vertical + 20
    elif vertical > 20:
        boom_speed = vertical - 20

    boom_motor.run(boom_speed * BOOMSPEED / 80)

        # control of the jib raising and lowering
    jib_speed = 0
    horizontal, vertical = controller.joystick_left()
    if horizontal < -20:
        jib_speed = horizontal + 20
    elif horizontal > 20:
        jib_speed = horizontal - 20

    jib_motor.run(jib_speed * JIBSPEED / 80)

    # control of the hook raising and lowering
    hook_speed = 0
    horizontal, vertical = controller.joystick_right()
    if vertical < -20:
        hook_speed = vertical + 20
    elif vertical > 20:
        hook_speed = vertical - 20

    hook_motor.run(hook_speed * HOOKSPEED / 80)

    # control of the superstructure turning
    rot_speed = 0
    horizontal, vertical = controller.joystick_right()
    if horizontal < -20:
        rot_speed = horizontal + 20
    elif horizontal > 20:
        rot_speed = horizontal - 20

    # left track control
    left_tr_speed = 0
    now_pressed = controller.buttons.pressed()
    if "X" in now_pressed:
        brake, acceleration = controller.triggers()
        if brake > 10:
            left_tr_speed -= brake
    else:
        brake, acceleration = controller.triggers()
        if brake > 10:
            left_tr_speed += brake
         
    # right track control
    right_tr_speed = 0
    now_pressed = controller.buttons.pressed()
    if "A" in now_pressed:
        brake, acceleration = controller.triggers()
        if acceleration > 10:
            right_tr_speed -= acceleration
    else:
        brake, acceleration = controller.triggers()
        if acceleration > 10:
            right_tr_speed += acceleration

    # Set the broadcast data and start broadcasting if not already doing so.
    data = (rot_speed, left_tr_speed, right_tr_speed)
    hub.ble.broadcast(data)