#
# !!!IMPORTANT!!! You need the LEGO Powered Up Remote Control 88010 to use this code. 
# Get it at https://www.lego.com/en-nl/product/remote-control-88010
#
# There are 2 steering modes. Normal steering and tank control. You can switch between them using the green button on the remote control.
#
from pybricks.hubs import TechnicHub
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Button, Color, Direction, Port, Side
from pybricks.tools import wait

# Set up hardware:
technicHub = TechnicHub()
technicHub.light.on(Color.RED)
remote = Remote()
technicHub.light.on(Color.GREEN)
remote.light.on(Color.GREEN)
motors = [Motor(Port.B), Motor(Port.A, Direction.COUNTERCLOCKWISE)]

# Main loop:
steeringmode = 0
steering = 0
STEERING_SPEED = 20 #1 to 20 for normal mode
while True:
    buttons = remote.buttons.pressed()


    if Button.CENTER in buttons:
        steeringmode = 1 - steeringmode
        if steeringmode == 0:
            remote.light.on(Color.GREEN)
            technicHub.light.on(Color.GREEN)
        else:
            remote.light.on(Color.YELLOW)
            technicHub.light.on(Color.YELLOW)
        while Button.CENTER in remote.buttons.pressed():
            wait(40) 
    

    if steeringmode == 0: #normalmode
        # Control speed with left side:
        speed = 0
        if Button.LEFT_PLUS in buttons:
            speed = 100
        elif Button.LEFT_MINUS in buttons:
            speed = -100

        # Control steering with right side:
        if Button.RIGHT_PLUS in buttons:
            steering = max(0, steering + STEERING_SPEED)
        elif Button.RIGHT_MINUS in buttons:
            steering = min(0, steering - STEERING_SPEED)
        else:
            steering = 0

        # Update motors:
        speed0 = max(-100, min(100, speed - steering))
        speed1 = max(-100, min(100, speed + steering))
        motors[0].dc(-speed0)
        motors[1].dc(-speed1)
    else: #tankmode
        speed = 100
        if Button.LEFT_PLUS in buttons:
            motors[0].dc(-speed)
        elif Button.LEFT_MINUS in buttons:
            motors[0].dc(speed)
        else:
            motors[0].dc(0)
        
        if Button.RIGHT_PLUS in buttons:
            motors[1].dc(-speed)
        elif Button.RIGHT_MINUS in buttons:
            motors[1].dc(speed)
        else:
            motors[1].dc(0)





    # Turn off using green button:
    #if Button.CENTER in buttons:
    #    technicHub.system.shutdown()

    wait(25)