from pybricks.hubs import PrimeHub
from pybricks.parameters import Color, Port
from pybricks.pupdevices import ColorSensor, Motor, UltrasonicSensor
from pybricks.tools import StopWatch, wait

print("\n" * 5)
print("TEST NORMALE (Pybricks)")

hub = PrimeHub()
PORTS = [Port.A, Port.B, Port.C, Port.D, Port.E, Port.F]


motori: list[Motor] = []
color_sensors: dict[Port, ColorSensor] = {}
distance_sensors: dict[Port, UltrasonicSensor] = {}

# Scoperta dispositivi (una sola istanza per porta)
for porta in PORTS:
    try:
        m = Motor(porta)
        motori.append(m)
        print(f"Motore trovato su porta {porta}")
        continue
    except Exception:
        pass
    try:
        cs = ColorSensor(porta)
        color_sensors[porta] = cs
        print(f"ColorSensor trovato su porta {porta}")
        continue
    except Exception:
        pass
    try:
        us = UltrasonicSensor(porta)
        distance_sensors[porta] = us
        print(f"UltrasonicSensor (distanza) trovato su porta {porta}")
        continue
    except Exception:
        pass

# Test motori
for m in motori:
    m.dc(100)
    wait(1000)
    m.stop()

# Letture sensori esterni
print("Valori dei sensori:")
for porta, cs in color_sensors.items():
    c = cs.color()
    nome = getattr(c, "name", "NONE") if c is not None else "NONE"
    print(f"ColorSensor su porta {porta}: colore rilevato {nome}")

for porta, us in distance_sensors.items():
    d = us.distance()
    if d is None:
        print(f"Ultrasonic su porta {porta}: distanza fuori portata")
    else:
        print(f"Ultrasonic su porta {porta}: distanza rilevata {d / 10:.1f} cm")

# Sensori integrati (IMU)
print("Valori dei sensori integrati nell'hub:")
tilt = hub.imu.tilt()
print(f"Tilt (roll, pitch): {tilt}")
gyro = hub.imu.angular_velocity()
print(f"Velocità angolare (x,y,z): {gyro}")


# ---------- FIX: parsing colori senza isinstance/issubclass --------------
def _color_member_or_none(name):
    try:
        return getattr(Color, name)
    except Exception:
        return None


def _parse_color(value):
    """
    Accetta Color o stringa e ritorna un membro di pybricks.parameters.Color
    senza usare isinstance/issubclass (compatibile con MicroPython).
    """
    # Caso: è già un "Color-like" (ha .name che corrisponde a un membro)
    try:
        nm = getattr(value, "name", None)
        if isinstance(nm, str) and _color_member_or_none(nm) is not None:
            return value
    except Exception:
        pass

    # Caso: stringa
    if isinstance(value, str):
        key = value.strip().replace(" ", "_").upper()
        # Sinonimi tra VIOLET/PURPLE (dipende dalla versione)
        if key in ("VIOLET", "PURPLE"):
            return _color_member_or_none("VIOLET") or _color_member_or_none("PURPLE")
        return _color_member_or_none(key)

    return None


# -------------------------------------------------------------------------


def wait_for_color(color, sensor_port=None, timeout=None, min_time=0):
    """
    Attende che un ColorSensor legga 'color'.
    - color: Color o str ('violet', 'white', ecc.)
    - sensor_port: Port o None per controllare tutti
    - timeout: secondi massimi (None = infinito)
    - min_time: stabilità minima in secondi
    """
    target = _parse_color(color)
    if target is None:
        print(f"Colore '{color}' non riconosciuto nell'enum Color.")
        return False

    if sensor_port is not None:
        sensori = (
            {sensor_port: color_sensors.get(sensor_port)}
            if sensor_port in color_sensors
            else {}
        )
    else:
        sensori = color_sensors

    if not sensori:
        print("Nessun ColorSensor disponibile sulle porte richieste.")
        return False

    sw = StopWatch()
    min_ms = int(min_time * 1000)
    timeout_ms = None if timeout is None else int(timeout * 1000)

    while True:
        if timeout_ms is not None and sw.time() > timeout_ms:
            print("Timeout raggiunto senza rilevare il colore.")
            return False

        for porta, cs in sensori.items():
            if cs is None:
                continue
            current = cs.color()
            nome = getattr(current, "name", "NONE") if current is not None else "NONE"
            print(f"Porta {porta}: Colore rilevato {nome}")

            if current == target:
                t0 = sw.time()
                while True:
                    cur2 = cs.color()
                    if cur2 != target:
                        break
                    if sw.time() - t0 >= min_ms:
                        print(
                            f"Colore {target.name} rilevato su porta {porta} per almeno {min_time} s."
                        )
                        return True
                    wait(100)
        wait(100)


# Sequenza principale
try:
    if wait_for_color("violet"):
        for m in motori:
            m.dc(100)
            m.dc(-100)

    if wait_for_color("white", min_time=2):
        for m in motori:
            m.stop()

    hub.speaker.beep()

finally:
    for m in motori:
        try:
            m.stop()
        except Exception:
            pass