import time
from machine import Pin, I2C

# Pines para motor de azimut
step_az = Pin(4, Pin.OUT)
dir_az = Pin(2, Pin.OUT)
# Pines para motor de altitud
step_alt = Pin(18, Pin.OUT)
dir_alt = Pin(5, Pin.OUT)

PASOS_POR_GRADO = 200 / 360  # Ajusta según tu motor/driver

def mover_motor(step_pin, dir_pin, pasos, sentido):
    dir_pin.value(sentido)
    for _ in range(abs(pasos)):
        step_pin.value(1)
        time.sleep_us(800)
        step_pin.value(0)
        time.sleep_us(800)

# --- LCD I2C ---
from lcd_api import LcdApi
from i2c_lcd import I2cLcd

i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)
lcd.putstr("Azimuth = A\nAltitud = B")

# --- Keypad 4x4 ---
from keypad import KeyPad

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

keypad = KeyPad(
    r1=13, r2=12, r3=14, r4=27,
    c1=26, c2=25, c3=33, c4=32,
    keys=Teclas
)
keypad.start()

# --- Lógica principal ---
last_key = None

azimuth = ""
altitude = ""
input_mode = -1   # 0: Azimut, 1: Altitud, -1: ninguno
can_input = False

azimuth_pos = 0
altitude_pos = 0

while True:
    try:
        key = keypad.get_key()
        if key and key != last_key:
            print("Tecla presionada:", key)

            # Permitir reactivar la entrada con A o B
            if key == 'A' or key == 'B':
                can_input = True
                if key == 'A':
                    input_mode = 0  # Azimut
                    lcd.clear()
                    lcd.move_to(0, 0)
                    lcd.putstr("Azimuth >")
                    lcd.move_to(0, 1)
                    lcd.putstr("Altitud: {}".format(altitude))
                    azimuth = ""
                elif key == 'B':
                    input_mode = 1  # Altitud
                    lcd.clear()
                    lcd.move_to(0, 0)
                    lcd.putstr("Azimuth: {}".format(azimuth))
                    lcd.move_to(0, 1)
                    lcd.putstr("Altitud >")
                    altitude = ""

            elif key == 'C':
                # Mover azimut a 0 (simulado)
                current_azimuth = 0
                steps_to_zero = -int(current_azimuth * PASOS_POR_GRADO)
                lcd.clear()
                lcd.move_to(0, 0)
                lcd.putstr("Moviendo a 0")
                lcd.move_to(0, 1)
                lcd.putstr("Azimuth: {}".format(current_azimuth))
                mover_motor(step_az, dir_az, abs(steps_to_zero), 1 if steps_to_zero > 0 else 0)
                azimuth_pos = 0
                time.sleep(2)
                lcd.clear()
                lcd.move_to(0, 0)
                lcd.putstr("Azimuth a 0")
                time.sleep(1)
                lcd.clear()
                lcd.move_to(0, 0)
                lcd.putstr("Azimuth: 0")
                lcd.move_to(0, 1)
                lcd.putstr("Altitud: {}".format(altitude_pos))

            elif key == 'D':
                # Establecer posición actual como nuevo punto 0
                azimuth_pos = 0
                altitude_pos = 0
                lcd.clear()
                lcd.move_to(0, 0)
                lcd.putstr("Nuevo punto 0")
                time.sleep(1)
                lcd.clear()
                lcd.move_to(0, 0)
                lcd.putstr("Azimuth: 0")
                lcd.move_to(0, 1)
                lcd.putstr("Altitud: 0")

            # Entrada manual de valores
            if can_input:
                if key == '#':
                    if input_mode != -1:
                        try:
                            az_val = float(azimuth) if azimuth else 0
                            alt_val = float(altitude) if altitude else 0

                            # Calcula los pasos actuales y los pasos destino
                            az_steps_target = int(az_val * PASOS_POR_GRADO)
                            alt_steps_target = int(alt_val * PASOS_POR_GRADO)

                            # Calcula la diferencia de pasos a mover (puede ser positiva o negativa)
                            delta_az_steps = az_steps_target - azimuth_pos
                            delta_alt_steps = alt_steps_target - altitude_pos

                            lcd.clear()
                            lcd.move_to(0, 0)
                            lcd.putstr("Moviendo...")

                            # Mueve azimut
                            if delta_az_steps != 0:
                                sentido_az = 1 if delta_az_steps > 0 else 0
                                mover_motor(step_az, dir_az, abs(delta_az_steps), sentido_az)
                                azimuth_pos = az_steps_target

                            # Mueve altitud
                            if delta_alt_steps != 0:
                                sentido_alt = 1 if delta_alt_steps > 0 else 0
                                mover_motor(step_alt, dir_alt, abs(delta_alt_steps), sentido_alt)
                                altitude_pos = alt_steps_target

                            lcd.clear()
                            lcd.move_to(0, 0)
                            lcd.putstr("Finalizado")
                            time.sleep(1)
                            lcd.clear()
                            lcd.move_to(0, 0)
                            lcd.putstr("Azimuth: {}".format(az_val))
                            lcd.move_to(0, 1)
                            lcd.putstr("Altitud: {}".format(alt_val))
                            can_input = False
                        except Exception as e:
                            lcd.clear()
                            lcd.putstr("Error entrada")
                            print("Error en entrada manual:", e)
                            time.sleep(1)
                            lcd.clear()
                elif key == '*':
                    if input_mode == 0 and '.' not in azimuth:
                        azimuth += '.'
                        lcd.move_to(9, 0)
                        lcd.putstr(azimuth)
                    elif input_mode == 1 and '.' not in altitude:
                        altitude += '.'
                        lcd.move_to(9, 1)
                        lcd.putstr(altitude)
                elif key.isdigit():
                    if input_mode == 0:
                        azimuth += key
                        lcd.move_to(9, 0)
                        lcd.putstr(azimuth)
                    elif input_mode == 1:
                        altitude += key
                        lcd.move_to(9, 1)
                        lcd.putstr(altitude)
        last_key = key
        time.sleep_ms(10)
    except Exception as e:
        print("Error en loop:", e)
        time.sleep(1)