from pybricks.pupdevices import Motor, Remote
from pybricks.hubs import TechnicHub
from pybricks.parameters import Button, Color, Direction, Port
from pybricks.tools import wait, StopWatch

# This drives like a car.
# Left +/- control going forwards/backwards, and right +/- turn.
# When the vehicle is moving turn is gradual, when it's stationary turn is faster.

remote = Remote()

hub = TechnicHub()

# TODO: I'm not sure what the max speed is, below is definitely a gross over
# exaggeration.  The only thing that's important is that TURN_SPEED be the same
# percentage of the max speed.
DRIVE_SPEED = 10000
TURN_SPEED = 650
DRIVE_ACCELERATION = 7000

# Initialize the motors.
right_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
left_motor = Motor(Port.B)

left_motor.control.limits(acceleration=DRIVE_ACCELERATION)
right_motor.control.limits(acceleration=DRIVE_ACCELERATION)

while True:
    # Check which buttons are pressed
    wait(10)
    pressed = remote.buttons.pressed()

    # Choose forward/backward and turn right/left based on pressed buttons
    left_speed = 0
    right_speed = 0
    pitch, roll = hub.imu.tilt()
    sign = -1 if abs(roll) > 90 else 1
    speed = sign*DRIVE_SPEED
    turn = sign*TURN_SPEED
    if Button.LEFT_PLUS in pressed or Button.LEFT_MINUS in pressed:
        # Going forwards/backwards
        left_speed = speed
        right_speed = speed
        
        # Turn slowly when moving
        turn_right = Button.RIGHT_PLUS in pressed
        turn_left = Button.RIGHT_MINUS in pressed
        if (turn_right and sign > 0) or (turn_left and sign < 0):
            right_speed = turn
        elif turn_right or turn_left:
            left_speed = turn

        if Button.LEFT_MINUS in pressed:
            left_speed *= -1
            right_speed *= -1
    else:
        # Not moving, fast turn
        if Button.RIGHT_PLUS in pressed:
            left_speed = DRIVE_SPEED
            right_speed = -DRIVE_SPEED
        if Button.RIGHT_MINUS in pressed:
            left_speed = -DRIVE_SPEED
            right_speed = DRIVE_SPEED

    # Activate the driving motors.
    left_motor.run(left_speed)
    right_motor.run(right_speed)

# Import required MicroPython libraries.
from usys import stdin
from uselect import poll
from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Button, Color, Direction, Port
from pybricks.tools import wait, StopWatch
from pybricks.hubs import TechnicHub

# Register hub and set light to soft white (gray)
hub = TechnicHub()
hub.light.on(Color.GRAY)

# Register the standard input so we can read keyboard presses.
keyboard = poll()
keyboard.register(stdin)

# Set defaults
left_speed = 0
right_speed = 0
function = "Blade up/down"
functionID = 1
key = None
state = "Idle"

# Default speed and acceleration. You can change
# them to make it more realistic.
SWITCH_SPEED = 720
DRIVE_SPEED = 100
DRIVE_ACCELERATION = 1000
FUNCTION_POWER = 100
MAX_LOAD = 75

# Initialize the motors.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
function_motor = Motor(Port.C)
switch_motor = Motor(Port.D)

left_motor.control.limits(acceleration=DRIVE_ACCELERATION)
right_motor.control.limits(acceleration=DRIVE_ACCELERATION)

# Define Functions
COMMANDS = {
    1: ("Blade up/down", 0, Color.BLUE),
    2: ("Blade tilt", 270, Color.RED),
    3: ("Ladder", 180, Color.YELLOW),
    4: ("Ripper", 90, Color.GREEN),
}

# Find the right end left endstop positions
right_end = switch_motor.run_until_stalled(500, duty_limit=50)
left_end = switch_motor.run_until_stalled(-500, duty_limit=50)

# The full switch motion ranges 270 degrees. Everything beyond that
# is play towards the endstops, equal in both directions. Since we
# are currently at the left endpoint, we can reset the angle
# accordingly. This way, the functions # are simply at the
# relative positions 0, 90, 180, 270.
switch_motor.reset_angle(-(right_end - left_end - 270) / 2)


def switch_target(target_angle):
    # Try up to 5 times.
    for i in range(5):
        # Keep track of how long we've tried.
        watch = StopWatch()
        switch_motor.run_target(SWITCH_SPEED, target, wait=True)
        # Wait until the stopwatch times out.
        while watch.time() < 2000:
            # We're done if the motor is on target, so exit
            # this function.
            if switch_motor.control.done():
                switch_motor.stop()
                return
            wait(10)

        # Otherwise, we got stuck, so try wiggling around to
        # release it.
        print("Getting unstuck.")
        switch_motor.run_target(SWITCH_SPEED, 0, wait=False)
        wait(1500)
        switch_motor.run_target(SWITCH_SPEED, 270, wait=False)
        wait(1500)
    # Always turn off motor to safe power
    switch_motor.stop()

# Print usage information
print(
    "\n\n\nKeys (FWD/REV/STP):\tw/s/x: Left track, e/d/c: Right Track, r/f/v: Both tracks full speed,",
    "\n\t\t\tq/a/z: Function, 0: Full stop, !: Shutdown.",
    "\nFuntions:\t\t1: Blade up/down 2: Blade tilt, 3: Ladder, 4: Ripper\n\n")

inc=0
while True:
    wait(10)

    # Stop function motor when jammed or blocked
    if function_motor.load() > MAX_LOAD or function_motor.load() < -MAX_LOAD:
        function_motor.stop()

    # Print statistical information
    inc += 1
    if inc % 50 == 0:
        LINE_CLEAR = '\x1b[2K'
        print(LINE_CLEAR,
            " State", state, "| Left: ",left_motor.speed(), left_motor.load(), "[", left_speed, 
            "], Right: ", right_motor.speed(), right_motor.load(), "[", right_speed,
            "] | Func.:", function,
            "| Bat:", hub.battery.voltage(), hub.battery.current(), end="\r"
            )
        inc = 0
    
    # Turn of motors when state it stopped for more then 5 seconds
    if state == "Stopped":
        if stopInc > 500:
            right_motor.stop()
            left_motor.stop()
            state = "Idle"
        stopInc += 1
    else:
        stopInc = 0

    # Restart loop of no keyboard input is given
    if not keyboard.poll(0):
        continue

    # Register key press
    if keyboard.poll(0):
        key = stdin.read(1)

    # Full stop
    if key == "0":
        state = "Idle"
        right_speed = 0
        left_speed = 0
        right_motor.stop()
        left_motor.stop()
        function_motor.stop()
        switch_motor.stop()

    # Shutdown hub
    if key == '!':
        print(LINE_CLEAR, "Good Bye!")
        wait(100)
        hub.system.shutdown()

    if key == "1" or key == "2" or key == "3" or key == "4":
        # Stop function motor and switch motor
        function_motor.stop()
        switch_motor.stop()

        # Go through the commands.
        for button, command in COMMANDS.items():
            # Check if the command has a matching button.
            if int(key) == button:
                # Now we can unpack the command.
                name, target, color = command
                # Execute command.
                switch_target(target)
                # Set color
                hub.light.on(color)
                # Update status info
                function = name
                functionID = button

    # Full speed ahead
    if key == "r":
        right_speed = 1000
        left_speed = 1000
        state = "Running"
    # Full speed reverse
    if key == "f":
        right_speed = -1000
        left_speed = -1000
        state = "Running"
    # Full stop
    if key == "v":
        right_speed = 0
        left_speed = 0
    # Increase left track speed
    if key == "w":
        left_speed += DRIVE_SPEED
        if left_speed > 1000:
            left_speed = 1000
        state = "Running"
    # Decrase left track speed
    if key == "s":
        left_speed -= DRIVE_SPEED
        if left_speed < -1000:
            left_speed = -1000
        state = "Running"
    # Stop left track
    if key == "x":
        left_speed = 0
    # Increate right track speed
    if key == "e":
        right_speed += DRIVE_SPEED
        if right_speed > 1000:
            right_speed = 1000
        state = "Running"
    # Decrease right track speed
    if key == "d":
        right_speed -= DRIVE_SPEED
        if right_speed < -1000:
            right_speed = -1000
        state = "Running"
    # Stop right track
    if key == "c":
        right_speed = 0
    # Function motor up
    if key == "q":
        function_motor.dc(-FUNCTION_POWER)
        if functionID == 1:
            MAX_LOAD = 150
        else:
            MAX_LOAD = 75 
    # Function motor down
    if key == "a":
        function_motor.dc(FUNCTION_POWER)
        MAX_LOAD = 170
    # Stop function motor
    if key == "z":
        function_motor.stop()

    # Activate the driving motors.
    if state != "Idle":
        left_motor.run(left_speed)
        right_motor.run(right_speed)

    # Set state to idle when speed is 0 on both tracks
    if left_speed == 0 and right_speed == 0 and state == "Running":
        state = "Stopped"

from pybricks.pupdevices import Motor, Remote
from pybricks.parameters import Port, Direction, Stop, Button
from pybricks.tools import wait

# Initialize the motors.
steer = Motor(Port.C)

# Initialize the motors with increased smoothness profile.
front = Motor(Port.A, Direction.COUNTERCLOCKWISE, profile=360)
rear = Motor(Port.B, Direction.COUNTERCLOCKWISE, profile=360)

# Lower the acceleration so the car starts and stops realistically.
front.control.limits(acceleration=1000)
rear.control.limits(acceleration=1000)

# Connect to the remote.
remote = Remote()

# Find the steering endpoint on the left and right.
# The middle is in between.
left_end = steer.run_until_stalled(-200, then=Stop.HOLD)
right_end = steer.run_until_stalled(200, then=Stop.HOLD)

# We are now at the right. Reset this angle to be half the difference.
# That puts zero in the middle.
steer.reset_angle((right_end - left_end) / 2)
steer.run_target(speed=200, target_angle=0, wait=False)

# Now we can start driving!
while True:
    # Check which buttons are pressed.
    pressed = remote.buttons.pressed()

    # Choose the steer angle based on the right controls.
    steer_angle = 0
    if Button.RIGHT_MINUS in pressed:
        steer_angle -= 75
    if Button.RIGHT_PLUS in pressed:
        steer_angle += 75

    # Steer to the selected angle.
    steer.run_target(500, steer_angle, wait=False)

    # Choose the drive speed based on the left controls.
    drive_speed = 0
    if Button.LEFT_PLUS in pressed:
        drive_speed += 1000
    if Button.LEFT_MINUS in pressed:
        drive_speed -= 1000

    # Apply the selected speed.
    front.run(drive_speed)
    rear.run(drive_speed)

    # Wait.
    wait(10)