from machine import Pin
from utime import sleep_us, ticks_ms

class Pins:
    # Sensores IR
    FRONT_CENTER = Pin(12, Pin.IN)
    LEFT_FRONT   = Pin(14, Pin.IN)
    FRONT_RIGHT  = Pin(13, Pin.IN)
    RIGHT        = Pin(2, Pin.IN)

    # Motores (simulados con LEDs)
    LEFT_FWD     = Pin(32, Pin.OUT)  # LED blanco
    LEFT_BWD     = Pin(33, Pin.OUT)  # LED verde
    RIGHT_FWD    = Pin(25, Pin.OUT)  # LED verde
    RIGHT_BWD    = Pin(26, Pin.OUT)  # LED blanco

    # Luces direccionales
    TURN_LEFT    = Pin(25, Pin.OUT)
    TURN_RIGHT   = Pin(19, Pin.OUT)
    class CarController:
    def __init__(self):
        self.last_direction = None

    def stop(self):
        """Detiene todos los motores y luces"""
        Pins.RIGHT_FWD.value(0)
        Pins.LEFT_FWD.value(0)
        Pins.RIGHT_BWD.value(0)
        Pins.LEFT_BWD.value(0)
        Pins.TURN_LEFT.value(0)
        Pins.TURN_RIGHT.value(0)
        self.last_direction = 'stop'

    def forward(self):
        """El carro hacia adelante"""
        Pins.RIGHT_FWD.value(1)
        Pins.LEFT_FWD.value(1)
        self.last_direction = 'forward'

    def backward(self):
        """El carro hacia atrás"""
        Pins.RIGHT_BWD.value(1)
        Pins.LEFT_BWD.value(1)
        self.last_direction = 'backward'

    def turn_right_90(self):
        """Gira 90 grados a la derecha usando tiempo"""
        self.stop()
        Pins.TURN_RIGHT.value(1)
        sleep(0.55)  # Ajusta este valor según tu carro
        self.stop()

    def turn_left_90(self):
        """Gira 90 grados a la izquierda usando tiempo"""
        self.stop()
        Pins.TURN_LEFT.value(1)
        sleep(0.55)  # Ajusta este valor según tu carro
        self.stop()

    def read_sensors(self):
        """Lee todos los sensores y devuelve valores"""
        return {
            'FRONT_CENTER': Pins.FRONT_CENTER.value(),
            'FRONT_LEFT': Pins.FRONT_LEFT.value(),
            'FRONT_RIGHT': Pins.FRONT_RIGHT.value(),
            'LEFT': Pins.LEFT.value(),
            'RIGHT': Pins.RIGHT.value(),
        }
        def interpret_sensors(self, cf, fc, fl, fr, l, r):
    """Toma decisiones basadas en los valores de los sensores"""
    print("Interpretando los valores de los sensores")

    # Todos los sensores detectan obstáculo
    if cf and fc and fl and fr and l and r:
        print("Obstáculo detectado en todas direcciones")
        self.backand()
        return

    # Pared frontal completa
    if cf and fc:
        print("Pared frontal detectada")
        if not l and r:  # Libre a la derecha
            self.turn_right_90()
            return
        elif l and not r:  # Libre a la izquierda
            self.turn_left_90()
            return
        self.backand()
        return

    # Intersección en T (frontal obstruido, laterales libres)
    if cf and fc and not fl and not fr and l and r:
        print("Intersección en T detectada")
        self.turn_right_90()
        return
        # Pasillo libre adelante
if not any((fc, fl, fr)):
    print("Pasillo libre - avanzando")
    self.forward()
    return

# Paredes laterales pero frontal libre
if not fc:
    if l and not r:  # Pared izquierda
        print("Siguiendo pared izquierda")
        self.forward()
    elif r and not l:  # Pared derecha
        print("Siguiendo pared derecha")
        self.forward()
    else:
        self.forward()

# Caso por defecto
print("Situación no especificada - avanzando con precaución")
self.forward()
# Función de prueba para simular sensores (eliminar en producción)
def mock_sensors():
    # Simula diferentes patrones de sensores
    patterns = [
        [0, 0, 0, 0],  # Todo libre
        [1, 0, 0, 0],  # Pared frontal
        [1, 1, 0, 0],  # Pared frontal e izquierda
        [1, 0, 1, 0],  # Pared frontal y derecha
        [0, 1, 0, 0],  # Pared izquierda
        [0, 0, 1, 0],  # Pared derecha
        [0, 1, 1, 0],  # Paredes laterales
        [1, 1, 1, 1],  # Intersección en T
    ]
    return patterns[ticks_ms() // 2000 % len(patterns)]

# Instancia del controlador
car = CarController()

# Bucle principal
def main():
    while True:
        # En producción usar car.read_sensors() en lugar de mock_sensors()
        sensor_values = mock_sensors()  # Cambiar a car.read_sensors() para pruebas reales
        print("Sensores:", sensor_values)
        car.interpret_sensors(*sensor_values)
        sleep_us(500000)  # 0.5 segundos

if __name__ == "__main__":
    main()