import cv2
import numpy as np
import RPi.GPIO as GPIO
import time

# Configuración de pines GPIO para los motores
class MotorController:
    def __init__(self):
        # Configurar modo de numeración GPIO
        GPIO.setmode(GPIO.BCM)

        # Definir pines para los motores
        self.motor_izq_adelante = 17
        self.motor_izq_atras = 18
        self.motor_der_adelante = 22
        self.motor_der_atras = 23

        # Configurar pines como salida
        GPIO.setup(self.motor_izq_adelante, GPIO.OUT)
        GPIO.setup(self.motor_izq_atras, GPIO.OUT)
        GPIO.setup(self.motor_der_adelante, GPIO.OUT)
        GPIO.setup(self.motor_der_atras, GPIO.OUT)

        # Inicializar PWM para control de velocidad
        self.pwm_izq = GPIO.PWM(self.motor_izq_adelante, 100)
        self.pwm_der = GPIO.PWM(self.motor_der_adelante, 100)
        self.pwm_izq.start(0)
        self.pwm_der.start(0)

    def mover(self, velocidad_izq, velocidad_der):
        """
        Control de movimiento de los motores
        velocidad: -100 a 100 (negativo para reversa)
        """
        # Motor izquierdo
        if velocidad_izq >= 0:
            self.pwm_izq.ChangeDutyCycle(velocidad_izq)
            GPIO.output(self.motor_izq_atras, GPIO.LOW)
        else:
            self.pwm_izq.ChangeDutyCycle(-velocidad_izq)
            GPIO.output(self.motor_izq_atras, GPIO.HIGH)

        # Motor derecho
        if velocidad_der >= 0:
            self.pwm_der.ChangeDutyCycle(velocidad_der)
            GPIO.output(self.motor_der_atras, GPIO.LOW)
        else:
            self.pwm_der.ChangeDutyCycle(-velocidad_der)
            GPIO.output(self.motor_der_atras, GPIO.HIGH)

    def detener(self):
        """Detiene ambos motores"""
        self.pwm_izq.ChangeDutyCycle(0)
        self.pwm_der.ChangeDutyCycle(0)
        GPIO.output(self.motor_izq_atras, GPIO.LOW)
        GPIO.output(self.motor_der_atras, GPIO.LOW)

def procesar_frame(frame):
    """
    Procesa el frame y detecta la línea negra
    Retorna: frame procesado y posición de la línea
    """
    # Convertir a escala de grises
    gris = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Aplicar desenfoque para reducir ruido
    desenfoque = cv2.GaussianBlur(gris, (5, 5), 0)

    # Binarización para detectar la línea negra
    _, binario = cv2.threshold(desenfoque, 60, 255, cv2.THRESH_BINARY_INV)

    # Región de interés (parte inferior de la imagen)
    altura = frame.shape[0]
    roi = binario[int(altura*0.6):altura, :]

    # Encontrar el centro de la línea
    momentos = cv2.moments(roi)
    if momentos["m00"] > 0:
        cx = int(momentos["m10"] / momentos["m00"])
        return binario, cx
    else:
        return binario, None

def seguidor_linea():
    # Inicializar cámara
    camara = cv2.VideoCapture(0)
    camara.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
    camara.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)

    # Inicializar controlador de motores
    motores = MotorController()

    # Parámetros de control
    velocidad_base = 50  # Velocidad base de los motores
    kp = 0.5  # Constante proporcional para el control

    try:
        while True:
            ret, frame = camara.read()
            if not ret:
                print("Error al capturar frame")
                break

            # Procesar frame y obtener posición de la línea
            frame_procesado, centro_linea = procesar_frame(frame)

            if centro_linea is not None:
                # Calcular error (distancia desde el centro)
                error = frame.shape[1]/2 - centro_linea

                # Calcular ajuste de velocidad usando control proporcional
                ajuste = kp * error

                # Aplicar velocidades a los motores
                velocidad_izq = max(min(velocidad_base + ajuste, 100), -100)
                velocidad_der = max(min(velocidad_base - ajuste, 100), -100)

                motores.mover(velocidad_izq, velocidad_der)
            else:
                # Si no se detecta la línea, detener motores
                motores.detener()

            # Mostrar frames para debugging
            cv2.imshow('Video Original', frame)
            cv2.imshow('Línea Detectada', frame_procesado)

            # Salir con 'q'
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break

    finally:
        # Limpieza
        motores.detener()
        camara.release()
        cv2.destroyAllWindows()
        GPIO.cleanup()

if __name__ == "__main__":
    seguidor_linea()