from machine import Pin, I2C
import ssd1306
from keypad_utp import KeyPad
from time import sleep

# === Configuración del hardware ===
# Configuración de la pantalla OLED
i2c = I2C(0, scl=Pin(22), sda=Pin(21))  # Pines GPIO22 (SCL) y GPIO21 (SDA)
oled = ssd1306.SSD1306_I2C(128, 64, i2c)  # Pantalla OLED 128x64

# Configuración del teclado matricial
Teclas = [
    '1', '2', '3', 'A',
    '4', '5', '6', 'B',
    '7', '8', '9', 'C',
    '*', '0', '#', 'D',
]
keypad = KeyPad(
    r1=19, r2=18, r3=5, r4=4,
    c1=13, c2=12, c3=14, c4=27,
    keys=Teclas
)
keypad.start()

# Configuración del motor 28BYJ-48
IN1 = Pin(26, Pin.OUT)
IN2 = Pin(25, Pin.OUT)
IN3 = Pin(33, Pin.OUT)
IN4 = Pin(32, Pin.OUT)

# Secuencia de pasos para el motor en modo medio paso
secuencia = [
    [1, 0, 0, 0],
    [1, 1, 0, 0],
    [0, 1, 0, 0],
    [0, 1, 1, 0],
    [0, 0, 1, 0],
    [0, 0, 1, 1],
    [0, 0, 0, 1],
    [1, 0, 0, 1],
]

# === Variables del sistema ===
pasos_por_revolucion = 512  # 360° en modo medio paso
angulo_actual = 0  # Posición actual del motor en grados
clave_ingresada = ""  # Para almacenar el ángulo ingresado

# === Funciones auxiliares ===
def mostrar_oled(linea1, linea2=""):
    """Muestra texto en la pantalla OLED."""
    oled.fill(0)
    oled.text(linea1, 0, 0)
    oled.text(linea2, 0, 16)
    oled.show()

def mover_motor(pasos):
    """
    Mueve el motor una cantidad de pasos.
    Pasos positivos: sentido horario.
    Pasos negativos: sentido antihorario.
    """
    global angulo_actual
    if pasos > 0:
        direccion = 1
    else:
        direccion = -1
        pasos = -pasos

    for _ in range(pasos):
        for paso in secuencia[::direccion]:
            IN1.value(paso[0])
            IN2.value(paso[1])
            IN3.value(paso[2])
            IN4.value(paso[3])
            sleep(0.002)  # Retardo entre pasos

    # Actualizar la posición actual del motor
    angulo_actual = (angulo_actual + (pasos * 360 / pasos_por_revolucion)) % 360

# === Mensaje inicial ===
mostrar_oled("Ingresa angulo", "0-360 y pulsa #")

# === Bucle principal ===
try:
    while True:
        key = keypad.get_key()

        if key:
            if key.isdigit():  # Si es un dígito, agregarlo a la clave ingresada
                clave_ingresada += key
                mostrar_oled(f"Angulo: {clave_ingresada}", "Pulsa # para OK")

            elif key == "*":  # Reiniciar la entrada
                clave_ingresada = ""
                mostrar_oled("Ingresa angulo", "0-360 y pulsa #")

            elif key == "#":  # Confirmar el ángulo ingresado
                if clave_ingresada:
                    angulo_deseado = int(clave_ingresada)
                    if 0 <= angulo_deseado <= 360:
                        # Calcular la diferencia de ángulo
                        diferencia = angulo_deseado - angulo_actual
                        # Convertir la diferencia a pasos
                        pasos = int(diferencia * pasos_por_revolucion / 360)
                        # Mover el motor
                        mover_motor(pasos)
                        # Mostrar la posición actual
                        mostrar_oled(f"Posicion: {int(angulo_actual)}", "")
                    else:
                        mostrar_oled("Angulo invalido", "0-360 permitido")
                    clave_ingresada = ""

        sleep(0.1)

finally:
    keypad.stop()