import socket
import network
from secrets import secrets
from utime import sleep, ticks_ms, ticks_diff
from machine import Pin, I2C
import ure
import binascii
import ssd1306
import framebuf

# === OLED Display ===
WIDTH = 128
HEIGHT = 64
i2c = I2C(1, scl=Pin(19), sda=Pin(18), freq=400000)
oled = ssd1306.SSD1306_I2C(WIDTH, HEIGHT, i2c)

# === Pines para motores paso a paso ===
STEP_X = Pin(2, Pin.OUT)
DIR_X = Pin(3, Pin.OUT)
STEP_Y = Pin(14, Pin.OUT)
DIR_Y = Pin(15, Pin.OUT)
STEP_Z = Pin(6, Pin.OUT)
DIR_Z = Pin(7, Pin.OUT)
STEP_Z2 = Pin(8, Pin.OUT)
DIR_Z2 = Pin(9, Pin.OUT)

# === Control del husillo ===
HUS = Pin(21, Pin.OUT)
gpio_state = False
HUS.value(0)

# === Parámetros de configuración ===
velocidad_ms = 10       # ms entre pasos
FACTOR_X = 7            # resolución eje X
FACTOR_YZ = 25          # resolución eje Y y Z

# === Mostrar pantalla inicial ===
def display_info(text, y_pos, clear=False):
    print(text)
    if clear:
        oled.fill(0)
    oled.text(text, 0, y_pos)
    oled.show()

logo = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x18, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x01, 0xc3, 0x11, 0x4e, 0x40, 0x43, 0x86, 0x22, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x03, 0x63, 0x19, 0x48, 0x40, 0xe6, 0xc6, 0x22, 0x58, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 
0x02, 0x25, 0x3a, 0x48, 0x40, 0xa4, 0x4a, 0x62, 0xc8, 0x00, 0x00, 0x80, 0x03, 0x00, 0x00, 0x00, 
0x02, 0x2d, 0x2a, 0x5e, 0xc1, 0x24, 0x5a, 0x46, 0x88, 0x00, 0x01, 0x00, 0x03, 0x80, 0x00, 0x00, 
0x02, 0x6f, 0x26, 0xd0, 0x81, 0xe4, 0xde, 0x44, 0x88, 0x00, 0x02, 0x00, 0x01, 0x80, 0x00, 0x00, 
0x07, 0xd9, 0xa6, 0x9c, 0xe2, 0x2f, 0xb3, 0x64, 0xf0, 0x00, 0x04, 0x00, 0x00, 0x40, 0x00, 0x00, 
0x01, 0x00, 0x00, 0x1c, 0xe0, 0x06, 0x00, 0x60, 0xc0, 0x00, 0x0c, 0x00, 0x01, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x02, 0x00, 0x00, 0x00, 
0x03, 0xc0, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x70, 0x38, 0x00, 0x02, 0x00, 0x00, 0x00, 
0x03, 0x9b, 0xcf, 0x32, 0x30, 0x7a, 0x27, 0x9f, 0x22, 0xf0, 0x38, 0x20, 0x01, 0x00, 0x00, 0x00, 
0x01, 0x92, 0x11, 0xba, 0x50, 0xc2, 0x28, 0xd1, 0x22, 0x80, 0x30, 0xc0, 0x01, 0x80, 0x00, 0x00, 
0x01, 0x16, 0x11, 0xaa, 0x50, 0xc3, 0xe8, 0xf1, 0x66, 0xe0, 0x30, 0x80, 0x00, 0xe0, 0x00, 0x00, 
0x01, 0x16, 0x11, 0x6e, 0xf0, 0x86, 0x48, 0xb1, 0x44, 0x80, 0x11, 0x80, 0x00, 0x60, 0x00, 0x00, 
0x01, 0x16, 0x13, 0x4c, 0x98, 0xc6, 0x49, 0xb2, 0x6d, 0x80, 0x13, 0x00, 0x00, 0x60, 0x00, 0x00, 
0x01, 0x03, 0x8c, 0x45, 0x08, 0x74, 0x46, 0x1f, 0x38, 0xe0, 0x17, 0x00, 0x00, 0x10, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x1f, 0x12, 0x00, 0x18, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x13, 0x80, 0x0c, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x13, 0x00, 0x06, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0x80, 0x03, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x83, 0x80, 0x03, 0x80, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc1, 0x80, 0x00, 0xc0, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc1, 0x00, 0x00, 0x60, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x30, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x80, 0x00, 0x18, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x80, 0x00, 0x1c, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x02, 0x00, 0x0c, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x04, 0x00, 0x0e, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x06, 0x00, 
0x00, 0x00, 0x00, 0x07, 0x81, 0x83, 0xe1, 0xe0, 0x00, 0x00, 0x00, 0x10, 0x0e, 0x40, 0x06, 0x00, 
0x00, 0x00, 0x00, 0x04, 0xc1, 0x80, 0x83, 0x00, 0x00, 0x00, 0x00, 0x10, 0x18, 0x20, 0x06, 0x00, 
0x00, 0x00, 0x00, 0x0c, 0xc2, 0xc1, 0x82, 0x00, 0x00, 0x00, 0x00, 0x10, 0x18, 0x00, 0x02, 0x00, 
0x00, 0x00, 0x00, 0x0c, 0x87, 0xc1, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x02, 0x00, 
0x00, 0x00, 0x00, 0x09, 0x87, 0xc1, 0x02, 0x00, 0x00, 0x00, 0x00, 0x11, 0x00, 0x02, 0x02, 0x00, 
0x00, 0x00, 0x00, 0x0f, 0x2c, 0x51, 0x13, 0x80, 0x00, 0x00, 0x03, 0xff, 0xf0, 0x0e, 0x02, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xe7, 0xff, 0xf6, 0x01, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7e, 0x00, 0x00, 0x00, 0x01, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x08, 0x01, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x05, 0x15, 0x7a, 0x03, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x02, 0x95, 0x3a, 0x03, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0xd2, 0x52, 0x06, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x30, 0x00, 0x00, 0x0e, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x03, 0xe0, 0x00, 0x00, 0x7c, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0xf8, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0xe0, 0x07, 0x80, 0x01, 0xf0, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x7e, 0x3f, 0xc0, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xfe, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
]

buffer = bytearray(logo)
fb = framebuf.FrameBuffer(buffer, WIDTH, HEIGHT, framebuf.MONO_HLSB)
oled.fill(0)
oled.blit(fb, 0, 0)
oled.show()

oled.fill(0)
oled.text("WiFi RP2040W & CNC", 5, 0)
oled.text("MEC458",10,25)
oled.text("Daniel Ticona Ch.", 0, 30)
oled.text("RU: 1780371", 0, 45)
oled.show()
sleep(3)

# === Conexión WiFi ===
ssid = secrets['ssid']
password = secrets['password']

wlan = network.WLAN(network.STA_IF)
wlan.active(True)
display_info("Iniciando WiFi...", 0, True)
sleep(3)

wlan.connect(ssid, password)
display_info(f"Conectando a: {ssid[:16]}", 10)
for _ in range(20):
    if wlan.isconnected():
        break
    sleep(0.5)

if not wlan.isconnected():
    display_info("Error conexión WiFi", 20)
else:
    ip = wlan.ifconfig()[0]
    channel = wlan.config('channel')
    mac = binascii.hexlify(wlan.config('mac'), ':').decode()
    ssid = wlan.config('essid')
    oled.fill(0)
    oled.text("Conexion exitosa!", 0, 0)
    oled.text(f"SSID: {ssid[:16]}", 0, 10)
    oled.text(f"Host: {wlan.config('hostname')}",0,20)
    oled.text(f"IP: {ip}", 0, 30)
    oled.show()

def unquote(string):
    parts = string.split('%')
    result = [parts[0]]
    for item in parts[1:]:
        if len(item) >= 2:
            try:
                char = binascii.unhexlify(item[:2]).decode('latin-1')
                result.append(char + item[2:])
                continue
            except:
                pass
        result.append('%' + item)
    return ''.join(result)

def mover_lineal(x, y, z):
    try:
        pasos_x = int(abs(x) * FACTOR_X)
        pasos_y = int(abs(y) * FACTOR_YZ)
        pasos_z = int(abs(z) * FACTOR_YZ)

        DIR_X.value(1 if x >= 0 else 0)
        DIR_Y.value(1 if y >= 0 else 0)
        DIR_Z.value(1 if z >= 0 else 0)
        DIR_Z2.value(1 if z >= 0 else 0)

        max_pasos = max(pasos_x, pasos_y, pasos_z, 1)
        inc_x = pasos_x / max_pasos
        inc_y = pasos_y / max_pasos
        inc_z = pasos_z / max_pasos
        acum_x = acum_y = acum_z = 0.0

        for _ in range(max_pasos):
            start_time = ticks_ms()

            STEP_X.value(0)
            STEP_Y.value(0)
            STEP_Z.value(0)
            STEP_Z2.value(0)

            acum_x += inc_x
            if acum_x >= 1.0:
                STEP_X.value(1)
                acum_x -= 1.0

            acum_y += inc_y
            if acum_y >= 1.0:
                STEP_Y.value(1)
                acum_y -= 1.0

            acum_z += inc_z
            if acum_z >= 1.0:
                STEP_Z.value(1)
                STEP_Z2.value(1)
                acum_z -= 1.0

            sleep(0.0005)

            STEP_X.value(0)
            STEP_Y.value(0)
            STEP_Z.value(0)
            STEP_Z2.value(0)

            elapsed = ticks_diff(ticks_ms(), start_time)
            if elapsed < velocidad_ms:
                sleep((velocidad_ms - elapsed) / 1000)

    except Exception as e:
        print(f"Error en mover_lineal: {str(e)}")

# === Intérprete de G-code ===
def interpretar_gcode(linea):
    try:
        linea = linea.strip()
        if not linea:
            return "Línea vacía"

        partes = linea.split()
        comando = partes[0].upper()

        if comando in ["G0", "G1"]:
            coords = {'X': 0.0, 'Y': 0.0, 'Z': 0.0}
            for p in partes[1:]:
                if len(p) > 1 and p[0].upper() in coords:
                    try:
                        coords[p[0].upper()] = float(p[1:])
                    except ValueError:
                        return f"Error: Valor no numérico en {p}"
            mover_lineal(coords['X'], coords['Y'], coords['Z'])
            return f"OK: Movimiento a X{coords['X']} Y{coords['Y']} Z{coords['Z']}"
        return f"Comando no soportado: {comando}"
    except Exception as e:
        return f"ERROR: {str(e)}"

# === Página Web HTML ===
def generar_pagina_web(mensaje=""):
    return f"""<!DOCTYPE html>
<html>
<head>
    <title>CNC Pico W</title>
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <style>
        body {{
            font-family: Arial, sans-serif;
            max-width: 800px;
            margin: auto;
            padding: 20px;
            background: #f5f5f5;
            color: #333;
        }}
        .panel {{
            background: white;
            border-radius: 8px;
            padding: 20px;
            margin-bottom: 20px;
            box-shadow: 0 2px 4px rgba(0,0,0,0.1);
        }}
        textarea {{
            width: 100%;
            height: 120px;
            padding: 10px;
            font-family: monospace;
            border-radius: 4px;
            border: 1px solid #ccc;
        }}
        button {{
            background: #0066cc;
            color: white;
            padding: 10px 15px;
            border: none;
            border-radius: 4px;
            cursor: pointer;
        }}
        .status {{
            background: #eee;
            padding: 10px;
            font-family: monospace;
            white-space: pre-wrap;
            border-radius: 4px;
        }}
    </style>
</head>
<body>
    <div class="panel">
        <h1>Control CNC Pico W</h1>
        <p><strong>Velocidad:</strong> {1000 // velocidad_ms} pasos/seg</p>
        <p><strong>Resolución X:</strong> {FACTOR_X} pasos/u</p>
        <p><strong>Resolución Y/Z:</strong> {FACTOR_YZ} pasos/u</p>
    </div>
    <div class="panel">
        <form action="/" method="POST">
            <h3>Enviar G-code</h3>
            <textarea name="gcode"></textarea>
            <br><button type="submit">Ejecutar</button>
        </form>
    </div>
    <div class="panel">
        <h3>Resultado</h3>
        <div class="status">{mensaje.replace('\n', '<br>')}</div>
    </div>
    <div class="panel">
        <h3>Husillo</h3>
        <form action="/husillo" method="POST">
            <button type="submit">{"APAGAR" if gpio_state else "ENCENDER"} Husillo</button>
        </form>
        <p>Estado actual: <strong>{"ENCENDIDO" if gpio_state else "APAGADO"}</strong></p>
    </div>
</body>
</html>"""

def iniciar_servidor():
    global gpio_state
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.bind(('', 80))
    s.listen(5)
    print("Servidor web iniciado")

    while True:
        conn, addr = s.accept()
        try:
            request = conn.recv(1024).decode('utf-8')
            resultado = ""

            if "POST /husillo" in request:
                gpio_state = not gpio_state
                HUS.value(gpio_state)
                resultado = f"Husillo {'activado' if gpio_state else 'desactivado'}"

            elif "POST /" in request:
                content_length = int(request.split('Content-Length: ')[1].split('\r\n')[0])
                post_data = request.split('\r\n\r\n')[1][:content_length]

                if 'gcode=' in post_data:
                    gcode_text = post_data.split('gcode=')[1]
                    gcode_text = unquote(gcode_text.replace('+', ' '))
                    lineas_resultado = []
                    for linea in gcode_text.split('\n'):
                        linea = linea.strip()
                        if linea:
                            res = interpretar_gcode(linea)
                            lineas_resultado.append(f"{linea} → {res}")
                    resultado = '\n'.join(lineas_resultado)

            response = generar_pagina_web(resultado)
            conn.send('HTTP/1.1 200 OK\r\nContent-Type: text/html\r\n\r\n')
            conn.sendall(response.encode('utf-8'))

        except Exception as e:
            print("Error en la conexión:", str(e))
        finally:
            conn.close()

try:
    iniciar_servidor()
except KeyboardInterrupt:
    print("\nServidor detenido")
except Exception as e:
    print(f"Error crítico: {str(e)}")