from pybricks.parameters import Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

from usys import stdin
from uselect import poll

class DriveBaseDiagnostics:
    def __init__(self, hub, motorclass, dbclass):
        self.PORT_MAP = {
            "A": Port.A,
            "B": Port.B,
            "C": Port.C,
            "D": Port.D,
            "E": Port.E,
            "F": Port.F,
            }
    def initializeDriveBase():
        leftmotorport = input("Enter the left motor port: ")
        left_motor = self.motorclass(port_map[leftmotorport], Direction.COUNTERCLOCKWISE)
        rightmotorport = input("Enter the right motor port: ")
        right_motor = self.motorclass(port_map[rightmotorport],Direction.CLOCKWISE)
        # DriveBase configuration
        WHEEL_DIAMETER = 68.8  # mm
        AXLE_TRACK = 180  # mm
        drive_base = self.dbclass(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
        drive_base.settings(600, 500, 300, 200)
        drive_base.use_gyro(True)
    def driveRobot():
        # 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")
                elif(key == "X")
                    break
                else:
                    print("That key doesn't do anything.")