from machine import Pin, PWM, ADC  # Importar ADC para lecturas analógicas
import utime

# Parámetros de control PID
Kp = 1.0
Ki = 0.0
Kd = 0.5

# Límites de PWM
pwmmax = 2**16 - 1

# Configuración de los pines de los motores
A1 = Pin(10, Pin.OUT)
A2 = Pin(11, Pin.OUT)
B1 = Pin(12, Pin.OUT)
B2 = Pin(13, Pin.OUT)

# Configuración de PWM para los motores
PWMA = PWM(Pin(25))
PWMA.freq(1000)
PWMB = PWM(Pin(15))
PWMB.freq(1000)

# Inicializa los pines de dirección de los motores
A1.value(0)
A2.value(0)
B1.value(0)
B2.value(0)

# Pines de control para el 4067
S0 = Pin(18, Pin.OUT)
S1 = Pin(19, Pin.OUT)
S2 = Pin(20, Pin.OUT)
S3 = Pin(21, Pin.OUT)

# Pin ADC para leer el valor analógico del sensor a través del 4067
ent_sen = ADC(Pin(26))

# Switch para seleccionar el tipo de línea
switch_linea = Pin(4, Pin.IN, Pin.PULL_DOWN)

# LED para indicar el estado del switch
status2_led = Pin(8, Pin.OUT)

# Botón de calibración
boton_calibracion = Pin(5, Pin.IN, Pin.PULL_DOWN)

# LED de estado para la calibración
status3_led = Pin(7, Pin.OUT)

# Número de sensores y muestras
NUM_SENSORES = 16
NUM_MUESTRAS = 50

# Valores de umbral para cada sensor
umbrales = [0] * NUM_SENSORES

# Clase para el control PID
class PID:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.prev_error = 0
        self.integral = 0

    def compute(self, setpoint, measured_value):
        error = setpoint - measured_value
        self.integral += error
        derivative = error - self.prev_error
        self.prev_error = error
        return self.Kp * error + self.Ki * self.integral + self.Kd * derivative

# Inicializa el controlador PID
pid_controller = PID(Kp, Ki, Kd)

# Función para seleccionar el sensor en el 4067
def multi_sensor(index):
    S0.value(index & 0x01)
    S1.value((index >> 1) & 0x01)
    S2.value((index >> 2) & 0x01)
    S3.value((index >> 3) & 0x01)

# Función para manejar los motores e imprimir los valores de PWM
def motores(izq, der):
    B1.value(1 if izq > 0 else 0)
    B2.value(0 if izq > 0 else 1)
    izq = abs(izq)
    
    A1.value(1 if der > 0 else 0)
    A2.value(0 if der > 0 else 1)
    der = abs(der)

    pwmavalue = int(der * pwmmax / 100)
    pwmbvalue = int(izq * pwmmax / 100)

    PWMB.duty_u16(pwmbvalue)
    PWMA.duty_u16(pwmavalue)

    # Imprimir los valores de PWM
    print(f"PWM Motor A: {pwmavalue}, PWM Motor B: {pwmbvalue}")

# Función para calibrar los sensores
def calibrar_sensores():
    global umbrales
    fondo_muestras = [0] * NUM_SENSORES
    linea_muestras = [0] * NUM_SENSORES

    # Esperar presionar botón para comenzar calibración del fondo
    print("Esperando para medir el fondo...")
    while not boton_calibracion.value():
        utime.sleep(0.1)
    
    # Calibración del fondo
    print("Midiendo el fondo...")
    for _ in range(NUM_MUESTRAS):
        status3_led.toggle()  # Parpadeo del LED
        for i in range(NUM_SENSORES):
            multi_sensor(i)
            fondo_muestras[i] += ent_sen.read_u16()
        utime.sleep(0.1)
    status3_led.on()  # Dejar LED encendido al finalizar

    # Promedio de las muestras del fondo
    fondo_muestras = [x // NUM_MUESTRAS for x in fondo_muestras]
    print("Fondo medido:", fondo_muestras)

    # Esperar presionar botón para comenzar calibración de la línea
    print("Esperando para medir la línea...")
    while not boton_calibracion.value():
        utime.sleep(0.1)
    
    # Calibración de la línea
    print("Midiendo la línea...")
    for _ in range(NUM_MUESTRAS):
        status3_led.toggle()  # Parpadeo del LED
        for i in range(NUM_SENSORES):
            multi_sensor(i)
            linea_muestras[i] += ent_sen.read_u16()
        utime.sleep(0.1)
    status3_led.on()  # Dejar LED encendido al finalizar

    # Promedio de las muestras de la línea
    linea_muestras = [x // NUM_MUESTRAS for x in linea_muestras]
    print("Línea medida:", linea_muestras)

    # Calcular umbral para cada sensor
    umbrales = [(fondo_muestras[i] + linea_muestras[i]) // 2 for i in range(NUM_SENSORES)]
    print("Umbrales calculados:", umbrales)

    # Esperar a que se presione el botón para comenzar operación normal
    print("Presiona el boton")
    while not boton_calibracion.value():
        utime.sleep(0.1)
    
    # Apagar LED de calibración
    status3_led.off()

# Función de control del robot
def control_loop():
    global umbrales
    setpoint = 0
    threshold = 2000  # Umbral temporal

    # Calibrar sensores antes de iniciar la operación normal
    calibrar_sensores()

    while True:
        med_sens = []

        # Verificar el estado del switch
        linea_blanca = switch_linea.value()
        if linea_blanca:
            status2_led.on()  # LED encendido si sigue línea blanca
            print("Modo: Línea blanca sobre fondo negro")
        else:
            status2_led.off()  # LED apagado si sigue línea negra
            print("Modo: Línea negra sobre fondo blanco")

        for i in range(NUM_SENSORES):
            multi_sensor(i)
            utime.sleep_us(1)
            value = ent_sen.read_u16()

            # Convertir valor analógico a digital usando el umbral calculado para cada sensor
            if linea_blanca:
                sensor_value = 1 if value < umbrales[i] else 0
            else:
                sensor_value = 1 if value > umbrales[i] else 0
            med_sens.append(sensor_value)
        
        # Imprimir los valores individuales de los sensores
        med_sens_str = ", ".join([f"S{i}: {med_sens[i]}" for i in range(NUM_SENSORES)])
        print(f"Valores de los sensores: {med_sens_str}")
        
        # Control de los motores basado en la posición de la línea
        # (Código similar al anterior para manejar el robot)
        
        utime.sleep(0.1)

# Iniciar el control
control_loop()