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

hub = PrimeHub()
class ColorSensorDiagnostics:
    def __init__(self):
        self.colorsensor = None
        self.port_map = {
            "A": Port.A,
            "B": Port.B,
            "C": Port.C,
            "D": Port.D,
            "E": Port.E,
            "F": Port.F,
        }
    def initializeColorSensor(self):
        valid_ports = {"A", "B", "C", "D", "E", "F"}
        while True:
            colorinput = input(
            "This will test your color sensor.\n"
            "Enter the port for the color sensor you would like to test (A, B, C, D, E, or F): "
            ).strip().upper()
            if colorinput not in valid_ports:
                print("Invalid port. Please enter A-F.")
                continue
            try:
                if self.colorsensor is None:
                    self.colorsensor = ColorSensor(self.port_map[colorinput])
                    print(f"Color Sensor initialized on port {colorinput}.")
                else:
                    print(f"Reusing existing color sensor on port {colorinput}.")
                break

            except OSError as e:
                if e.errno == 16:  # EBUSY
                    print(f"Port {colorinput} is already in use. Reusing existing color sensor.")
                    break
                else:
                    print(f"Error initializing color sensor on port {colorinput}: {e}")
                    print("Make sure a color sensor is actually connected to this port.")
                    self.colorsensor = None
        self.colorsensor.detectable_colors(Color.RED, Color.YELLOW, Color.GREEN, Color.BLUE, Color.WHITE, Color.NONE)
    def printOutput(self):
        while True:
            print("HSV output:", self.colorsensor.hsv)
            print("Detected color:", self.colorsensor.color())