from machine import Pin, SPI, SoftI2C
import time
import framebuf 

# ==========================================
# CONFIGURAÇÃO DE PINOS
# ==========================================
# Botão e LED RGB Externo
sw_pin = Pin(2, Pin.IN, Pin.PULL_DOWN)
led_r = Pin(28, Pin.OUT)
led_g = Pin(27, Pin.OUT)
led_b = Pin(22, Pin.OUT)

# LED Interno do Raspberry Pi Pico W (Para garantir que você veja algo ligar)
try:
    led_interna = Pin("LED", Pin.OUT)
except:
    led_interna = Pin(25, Pin.OUT) # Fallback caso o Wokwi use a versão sem "W"

# SPI Display ILI9341 (SPI0 via Hardware)
spi = SPI(0, baudrate=40000000, sck=Pin(18), mosi=Pin(19), miso=Pin(16))
cs = Pin(26, Pin.OUT)
dc = Pin(21, Pin.OUT)

# Pinos extras do Display
rst = Pin(15, Pin.OUT)
lcd_led = Pin(14, Pin.OUT)

# Touch Capacitivo (I2C)
touch_i2c = SoftI2C(scl=Pin(17), sda=Pin(20))

# Acelerômetros MPU6050 (I2C)
imu_i2c = [
    SoftI2C(scl=Pin(3), sda=Pin(4)),   # IMU 1
    SoftI2C(scl=Pin(5), sda=Pin(7)),   # IMU 2
    SoftI2C(scl=Pin(6), sda=Pin(8)),   # IMU 3
    SoftI2C(scl=Pin(9), sda=Pin(10))   # IMU 4
]

# ==========================================
# CLASSES MINIMALISTAS (DRIVERS)
# ==========================================
class MPU6050:
    def __init__(self, i2c):
        self.i2c = i2c
        self.addr = 0x68
        try:
            self.i2c.writeto_mem(self.addr, 0x6B, b'\x00') # Wake up
        except:
            pass

    def get_accel(self):
        try:
            data = self.i2c.readfrom_mem(self.addr, 0x3B, 6)
            vals = []
            for i in range(3):
                val = (data[i*2] << 8) | data[i*2+1]
                if val > 32767: val -= 65536
                vals.append(val / 16384.0) # Retorna em Gs
            return vals
        except:
            return [0, 0, 0]

class FT6206:
    def __init__(self, i2c):
        self.i2c = i2c
        self.addr = 0x38

    def get_touch(self):
        try:
            num = self.i2c.readfrom_mem(self.addr, 0x02, 1)[0]
            if num in [1, 2]:
                data = self.i2c.readfrom_mem(self.addr, 0x03, 4)
                x = 320 - (((data[0] & 0x0F) << 8) | data[1])
                y = 240 - (((data[2] & 0x0F) << 8) | data[3])
                return x, y
        except:
            pass
        return None

# ==========================================
# FUNÇÕES DE TELA E INTERFACE
# ==========================================
def set_led(r, g, b):
    # Liga o LED RGB Externo
    led_r.value(r); led_g.value(g); led_b.value(b)
    # Acende o LED da Placa junto com a cor Verde do seu RGB para debug
    led_interna.value(g) 

def init_display():
    lcd_led.value(1)
    rst.value(1); time.sleep(0.05)
    rst.value(0); time.sleep(0.05)
    rst.value(1); time.sleep(0.1)

    cmds = [
        (0x01, None), (0x28, None), (0x3A, b'\x55'),
        (0x36, b'\x28'), (0x11, None),
    ]
    for cmd, data in cmds:
        cs.value(0); dc.value(0); spi.write(bytearray([cmd]))
        if data: dc.value(1); spi.write(data)
        cs.value(1)
        time.sleep(0.1)
    
    cs.value(0); dc.value(0); spi.write(bytearray([0x29])); cs.value(1)

def set_window(x0, y0, x1, y1):
    cs.value(0); dc.value(0)
    spi.write(bytearray([0x2A])); dc.value(1); spi.write(x0.to_bytes(2, 'big') + x1.to_bytes(2, 'big'))
    dc.value(0); spi.write(bytearray([0x2B])); dc.value(1); spi.write(y0.to_bytes(2, 'big') + y1.to_bytes(2, 'big'))
    dc.value(0); spi.write(bytearray([0x2C])); dc.value(1)

def fill_rect(x, y, w, h, color):
    set_window(x, y, x + w - 1, y + h - 1)
    pixel = bytes([color >> 8, color & 0xFF])
    total_pixels = w * h
    chunk_size = 1024
    
    if total_pixels <= chunk_size:
        spi.write(pixel * total_pixels)
    else:
        chunk_buf = pixel * chunk_size
        for _ in range(total_pixels // chunk_size): spi.write(chunk_buf)
        resto = total_pixels % chunk_size
        if resto > 0: spi.write(pixel * resto)
    cs.value(1)

def draw_text(text, x, y, color=0xFFFF, bg=0x0000):
    w = len(text) * 8
    h = 8
    buf = bytearray(w * h * 2)
    fb = framebuf.FrameBuffer(buf, w, h, framebuf.RGB565)
    
    fb.fill(bg)
    fb.text(text, 0, 0, color)
    
    for i in range(0, len(buf), 2):
        buf[i], buf[i+1] = buf[i+1], buf[i]
        
    set_window(x, y, x + w - 1, y + h - 1)
    spi.write(buf)
    cs.value(1)

def draw_hud(sensor_idx, colors):
    fill_rect(0, 0, 320, 16, 0x0000)
    
    # ATUALIZADO: Mostra IMU em vez de SENSOR
    draw_text(f"IMU {sensor_idx + 1}", 5, 4, 0xFFFF) 
    
    draw_text("X:", 190, 4, 0xFFFF); fill_rect(210, 4, 8, 8, colors[0])
    draw_text("Y:", 230, 4, 0xFFFF); fill_rect(250, 4, 8, 8, colors[1])
    draw_text("Z:", 270, 4, 0xFFFF); fill_rect(290, 4, 8, 8, colors[2])

# ==========================================
# LÓGICA PRINCIPAL
# ==========================================
print("Iniciando hardware...")
set_led(1, 1, 0) # Amarelo: Boot
init_display()
fill_rect(0, 0, 320, 240, 0x0000) 
set_led(1, 0, 0) # Vermelho: Aguardando

sensors = [MPU6050(i2c) for i2c in imu_i2c]
touch = FT6206(touch_i2c)

current_sensor_idx = 0
system_on = False
touch_pressed = False # Variável de controle do Click

colors = [0xF800, 0x07E0, 0x001F] 
graph_x_pos = 0

print("Sistema pronto. Ligue a chave (SW1).")

while True:
    is_on = sw_pin.value() == 1
    
    if is_on and not system_on:
        system_on = True
        set_led(1, 1, 0) # Amarelo: Transição
        fill_rect(0, 0, 320, 240, 0x0000)
        draw_hud(current_sensor_idx, colors)
        graph_x_pos = 0
        set_led(0, 1, 0) # Verde: Rodando
        print("Sistema Ligado.")
        
    elif not is_on and system_on:
        system_on = False
        set_led(1, 0, 0) # Vermelho: Desligado
        fill_rect(0, 0, 320, 240, 0x0000) 
        print("Sistema Desligado.")

    if not system_on:
        time.sleep(0.1)
        continue

    # 2. Lógica do Touch (Click Simples)
    t = touch.get_touch()
    if t:
        if not touch_pressed: # Só entra aqui no momento exato do clique
            touch_pressed = True
            current_sensor_idx = (current_sensor_idx + 1) % 4
            
            print(f"Mudou para o IMU {current_sensor_idx + 1}")
            fill_rect(0, 16, 320, 224, 0x0000) 
            draw_hud(current_sensor_idx, colors) 
            graph_x_pos = 0 
    else:
        touch_pressed = False # Libera para um novo clique quando soltar a tela

    # 3. Leitura e Gráfico
    ax, ay, az = sensors[current_sensor_idx].get_accel()
    
    map_y = lambda val: int(min(max((val + 2) * (224 / 4), 0), 223)) + 16
    
    fill_rect(graph_x_pos, 16, 2, 224, 0x0000)
    
    fill_rect(graph_x_pos, map_y(ax), 2, 2, colors[0]) 
    fill_rect(graph_x_pos, map_y(ay), 2, 2, colors[1]) 
    fill_rect(graph_x_pos, map_y(az), 2, 2, colors[2]) 
    
    graph_x_pos += 2
    if graph_x_pos >= 320:
        graph_x_pos = 0 

    time.sleep(0.01)