from machine import Pin, ADC, PWM
import time

# Configuración de Pines
class MinisumoRobot:
    def __init__(self):
        # Sensores QTR
        self.qtr_left = Pin(0, Pin.IN)
        self.qtr_right = Pin(1, Pin.IN)
        
        # Sensor SHARP
        self.sharp_sensor = ADC(Pin(26))
        
        # Motores - Driver L298N
        self.motor_left_forward = PWM(Pin(2))
        self.motor_left_backward = PWM(Pin(3))
        self.motor_right_forward = PWM(Pin(4))
        self.motor_right_backward = PWM(Pin(5))
        
        # Configuración PWM
        for motor in [self.motor_left_forward, self.motor_left_backward, 
                      self.motor_right_forward, self.motor_right_backward]:
            motor.freq(1000)
            motor.duty_u16(0)
    
    def read_qtr_sensors(self):
        """Lee los sensores de línea blanca"""
        return {
            'left': self.qtr_left.value(), 
            'right': self.qtr_right.value()
        }
    
    def read_sharp_sensor(self):
        """Lee la distancia del sensor SHARP"""
        return self.sharp_sensor.read_u16()
    
    def move_forward(self, speed=32767):
        """Movimiento hacia adelante"""
        self.motor_left_forward.duty_u16(speed)
        self.motor_left_backward.duty_u16(0)
        self.motor_right_forward.duty_u16(speed)
        self.motor_right_backward.duty_u16(0)
    
    def move_backward(self, speed=32767):
        """Movimiento hacia atrás"""
        self.motor_left_forward.duty_u16(0)
        self.motor_left_backward.duty_u16(speed)
        self.motor_right_forward.duty_u16(0)
        self.motor_right_backward.duty_u16(speed)
    
    def turn_left(self, speed=32767):
        """Girar a la izquierda"""
        self.motor_left_forward.duty_u16(0)
        self.motor_left_backward.duty_u16(speed)
        self.motor_right_forward.duty_u16(speed)
        self.motor_right_backward.duty_u16(0)
    
    def turn_right(self, speed=32767):
        """Girar a la derecha"""
        self.motor_left_forward.duty_u16(speed) 
        self.motor_left_backward.duty_u16(0)
        self.motor_right_forward.duty_u16(0)
        self.motor_right_backward.duty_u16(speed)
    
    def stop(self):
        """Detener todos los motores"""
        for motor in [self.motor_left_forward, self.motor_left_backward, 
                      self.motor_right_forward, self.motor_right_backward]:
            motor.duty_u16(0)
    
    def combat_strategy(self):
        """Estrategia básica de combate"""
        qtr_state = self.read_qtr_sensors()
        sharp_distance = self.read_sharp_sensor()
        
        # Lógica anti-caída de ring
        if qtr_state['left'] == 1 or qtr_state['right'] == 1:
            self.move_backward()
            time.sleep(0.5)
            self.turn_right()
            time.sleep(0.3)
        
        # Lógica de detección de oponente
        if sharp_distance < 5000:  # Umbral ajustable
            self.move_forward()
        else:
            self.turn_right()

# Inicialización y bucle principal
robot = MinisumoRobot()
 
while True:
    robot.combat_strategy()
    time.sleep(0.1)