# Seguidor de Linha — ESP32 (MicroPython) + driver TB6612 + régua QTR-8RC
# Controle PID com filtro de média móvel e recuperação de linha perdida.
# (código original; comentários traduzidos para o português)
from machine import Pin, PWM
import time

# ------------------ Pinos dos Motores (TB6612) ------------------
PWMA = PWM(Pin(4), freq=5000, duty=0)   # PWM do motor A
PWMB = PWM(Pin(5), freq=5000, duty=0)   # PWM do motor B

AIN1 = Pin(18, Pin.OUT)
AIN2 = Pin(19, Pin.OUT)
BIN1 = Pin(21, Pin.OUT)
BIN2 = Pin(22, Pin.OUT)

# ------------------ Pinos dos Sensores QTR-8RC ------------------
sensors = [
    Pin(13, Pin.IN), Pin(12, Pin.IN), Pin(14, Pin.IN), Pin(27, Pin.IN),
    Pin(26, Pin.IN), Pin(25, Pin.IN), Pin(33, Pin.IN), Pin(32, Pin.IN)
]
NUM_SENSORS = len(sensors)

# ------------------ Parâmetros do PID ------------------
cruzero = 300  # velocidade base (0–1023)
Kp = 0.2
Kd = 10.0
Ki = 0.005

PRO = 0
DER = 0
INT = 0
LAST = 0

# ------------------ Filtro dos sensores ------------------
history = [3500] * 5  # histórico inicial para a média móvel

def read_qtr_digital():
    """Devolve a posição com base nos sensores digitais (0 ou 1)."""
    values = [s.value() for s in sensors]
    weighted_sum = 0
    total = 0
    for i, v in enumerate(values):
        weighted_sum += v * i * 1000
        total += v
    if total == 0:
        return 3500  # centro por padrão
    return int(weighted_sum / total)

def read_qtr_filtered():
    """Filtra a leitura dos sensores usando média móvel."""
    pos = read_qtr_digital()
    history.pop(0)
    history.append(pos)
    return sum(history) // len(history)

# ------------------ Controle dos motores ------------------
def set_motor(left, right):
    """Controla os motores esquerdo e direito com valores de -1023 a 1023."""
    # Motor A (esquerdo)
    if left >= 0:
        AIN1.value(1); AIN2.value(0); PWMA.duty(min(left, 1023))
    else:
        AIN1.value(0); AIN2.value(1); PWMA.duty(min(-left, 1023))
    # Motor B (direito)
    if right >= 0:
        BIN1.value(1); BIN2.value(0); PWMB.duty(min(right, 1023))
    else:
        BIN1.value(0); BIN2.value(1); PWMB.duty(min(-right, 1023))

# ------------------ Programa principal ------------------
print("Iniciando o seguimento de linha...")
time.sleep(2)

last_time = time.ticks_ms()

while True:
    now = time.ticks_ms()
    dt = max((now - last_time) / 1000, 0.001)  # evita divisão por zero
    last_time = now

    # Leitura de posição filtrada
    pos = read_qtr_filtered()
    PRO = pos - 3500

    # Linha perdida -> giros suaves para reencontrar
    if PRO < -3500:
        vel_loss = int(cruzero * 0.8)
        set_motor(-vel_loss, vel_loss)
        INT = 0  # zera o integral
    elif PRO > 3500:
        vel_loss = int(cruzero * 0.8)
        set_motor(vel_loss, -vel_loss)
        INT = 0
    else:
        # PID
        DER = (PRO - LAST) / dt
        INT += PRO * dt
        INT = max(min(INT, 3000), -3000)  # anti-windup

        vel = (PRO * Kp) + (DER * Kd) + (INT * Ki)
        vel = max(min(vel, cruzero), -cruzero)

        # Ajuste dinâmico da velocidade base nas curvas
        baseSpeed = cruzero
        if abs(PRO) > 1500:
            baseSpeed = int(cruzero * 0.7)

        left = baseSpeed - vel
        right = baseSpeed + vel
        set_motor(int(left), int(right))

        LAST = PRO

    time.sleep_ms(5)