import machine
import time
from machine import Pin, PWM, SPI

# ==========================================
# CONFIGURAÇÃO DOS PINOS (Ajuste se necessário)
# ==========================================

# 1. LEDs
led_verde = Pin(12, Pin.OUT)  # Exemplo de pino para o LED 1
led_vermelho = Pin(14, Pin.OUT)  # Exemplo de pino para o LED 2

# 2. Servomotores (Frequência padrão de 50Hz para servos)
servo1 = PWM(Pin(13), freq=50)  # Servo superior
servo2 = PWM(Pin(15), freq=50)  # Servo inferior

def mover_servo(servo, angulo):
    # Converte ângulo (0 a 180) para o ciclo de trabalho do PWM (duty)
    # Tipicamente entre 20 e 120 no MicroPython (0.5ms a 2.5ms)
    duty = int(20 + (angulo / 180) * 100)
    servo.duty(duty)

# 3. Sensor Ultrassônico (HC-SR04)
trig = Pin(26, Pin.OUT)
echo = Pin(25, Pin.IN)

def ler_distancia():
    trig.off()
    time.sleep_us(2)
    trig.on()
    time.sleep_us(10)
    trig.off()
    
    while echo.value() == 0:
        pulso_inicio = time.ticks_us()
    while echo.value() == 1:
        pulso_fim = time.ticks_us()
        
    duracao = time.ticks_diff(pulso_fim, pulso_inicio)
    # Calcula a distância em centímetros (velocidade do som = 340m/s)
    distancia = (duracao * 0.0343) / 2
    return distancia

# 4. Teclado Matricial 4x4
linhas = [Pin(19, Pin.OUT), Pin(18, Pin.OUT), Pin(5, Pin.OUT), Pin(17, Pin.OUT)]
colunas = [Pin(16, Pin.IN, Pin.PULL_DOWN), Pin(4, Pin.IN, Pin.PULL_DOWN), 
           Pin(2, Pin.IN, Pin.PULL_DOWN), Pin(15, Pin.IN, Pin.PULL_DOWN)]

teclas = [
    ['1', '2', '3', 'A'],
    ['4', '5', '6', 'B'],
    ['7', '8', '9', 'C'],
    ['*', '0', '#', 'D']
]

def ler_teclado():
    for i, linha in enumerate(linhas):
        linha.on()
        for j, coluna in enumerate(colunas):
            if coluna.value() == 1:
                linha.off()
                return teclas[i][j]
        linha.off()
    return None

# 5. Inicialização do Display ILI9341 (SPI)
# Nota: Para o display funcionar, você precisará salvar o arquivo 'ili9341.py' 
# na aba de arquivos do seu Wokwi.
try:
    import ili9341
    # Pinos padrão SPI do ESP32: SCK=18, MOSI=23, MISO=19
    spi = SPI(1, baudrate=40000000, sck=Pin(18), mosi=Pin(23))
    display = ili9341.ILI9341(spi, cs=Pin(22), dc=Pin(21), rst=Pin(27))
    display.fill(ili9341.color565(0, 0, 0)) # Limpa a tela (Preto)
    display_disponivel = True
except ImportError:
    print("Aviso: Biblioteca ili9341 não encontrada no simulador.")
    display_disponivel = False

# ==========================================
# LOOP PRINCIPAL DO SISTEMA
# ==========================================

print("Sistema Inicializado...")
mover_servo(servo1, 0)
mover_servo(servo2, 0)

while True:
    # 1. Verifica o Teclado
    tecla = ler_teclado()
    if tecla:
        print("Tecla pressionada:", tecla)
        # Exemplo de lógica: se pressionar 'A', move o servo 1
        if tecla == 'A':
            mover_servo(servo1, 90)
            led_verde.on()
            time.sleep(0.5)
            led_verde.off()
        elif tecla == 'B':
            mover_servo(servo1, 0)
            
        time.sleep(0.3) # Debounce do teclado

    # 2. Verifica a Distância do Ultrassônico
    dist = ler_distancia()
    if dist < 20: # Se algo estiver a menos de 20cm
        led_vermelho.on()
        mover_servo(servo2, 90) # Abre ou aciona o segundo servo
    else:
        led_vermelho.off()
        mover_servo(servo2, 0)

    # 3. Atualiza dados no display (se configurado)
    if display_disponivel:
        display.text(f"Dist: {dist:.1f} cm", 10, 10, ili9341.color565(255, 255, 255))
        if tecla:
            display.text(f"Tecla: {tecla}", 10, 30, ili9341.color565(0, 255, 0))

    time.sleep(0.1)