from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

from usys import stdin
from uselect import poll

hub = PrimeHub()
port_map = {
    "A": Port.A,
    "B": Port.B,
    "C": Port.C,
    "D": Port.D,
    "E": Port.E,
    "F": Port.F,
    }
leftmotorport = input("Enter the left motor port: ")
left_motor = Motor(port_map[leftmotorport], Direction.COUNTERCLOCKWISE)
rightmotorport = input("Enter the right motor port: ")
right_motor = Motor(port_map[rightmotorport],Direction.CLOCKWISE) # Specify default direction

# DriveBase configuration
WHEEL_DIAMETER = 68.8  # mm (adjust for your wheels)
AXLE_TRACK = 180  # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)
# Register the standard input so we can read keyboard presses.
keyboard = poll()
keyboard.register(stdin)

while True:
    # Check if a key has been pressed.
    if keyboard.poll(0):
        # Read the key and print it.
        key = stdin.read(1)
        print("You pressed:", key)
        if(key == "W"):
            print("Insert robot go forward code here")
        elif(key == "A"):
            print("Insert robot go left code here")
        elif(key == "S"):
            print("Insert robot go backwards code here")
        elif(key == "D"):
            print("Insert robot go right code here")
        else:
            print("That key doesn't do anything.")