""" 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)
arm_motor = Motor(Port.B, positive_direction=Direction.COUNTERCLOCKWISE)
bucket_motor = Motor(Port.C, positive_direction=Direction.CLOCKWISE)
scoop_motor = Motor(Port.D, positive_direction=Direction.CLOCKWISE)

# motors settings
BOOMSPEED = 1000
BUCKETSPEED = 100
ARMSPEED = 1000
SCOOPSPEED = 1000

hub.light.on(Color.GREEN)

while True:


    # # 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
    # bucket_speed = 0
    # horizontal, vertical = controller.joystick_left()
    # if horizontal < -20:
    #     bucket_speed = horizontal + 20
    # elif horizontal > 20:
    #     bucket_speed = horizontal - 20

    # bucket_motor.run(bucket_speed * BUCKETSPEED / 80)

    # # control of arm
    # arm_speed = 0
    # horizontal, vertical = controller.joystick_right()
    # if vertical < -20:
    #     arm_speed = vertical + 20
    # elif vertical > 20:
    #     arm_speed = vertical - 20

    # arm_motor.run(arm_speed * ARMSPEED / 80)

    # # control of scoop
    # scoop_speed = 0
    # horizontal, vertical = controller.joystick_right()
    # if horizontal < -20:
    #     scoop_speed = vertical + 20
    # elif horizontal > 20:
    #     scoop_speed = vertical - 20

    # scoop_motor_motor.run(scoop_speed * SCOOPSPEED / 80)

    # control of the superstructure turning
    rot_speed = 0
    now_pressed = controller.buttons.pressed()
    if controller.button.RIGHT:
        rot_speed = ROTATESPEED
    elif controller.button.LEFT:
        rot_speed = ROTATESPEED * -1

        

    # 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)