from machine import Pin, PWM, ADC, I2C, time_pulse_us
from time import ticks_ms, ticks_us, ticks_diff, sleep_ms, sleep_us

# ==========================================
# PRACTICA 7 - CELDA MECATRONICA
# RASPBERRY PI PICO W - MICROPYTHON
# ==========================================

# SENSOR ULTRASONICO HC-SR04
trig = Pin(16, Pin.OUT)
echo = Pin(17, Pin.IN)
trig.off()

# SERVOMOTOR
servo = PWM(Pin(15))
servo.freq(50)

# CONTROLADOR A4988
step = Pin(2, Pin.OUT)
direccion = Pin(3, Pin.OUT)
enable = Pin(4, Pin.OUT)
step.off()
enable.on()

# POTENCIOMETRO
pot = ADC(Pin(26))

# ENCODER
clk = Pin(10, Pin.IN, Pin.PULL_UP)
dt = Pin(11, Pin.IN, Pin.PULL_UP)
sw = Pin(12, Pin.IN, Pin.PULL_UP)

# INDICADORES LED
verde = Pin(18, Pin.OUT)
amarillo = Pin(19, Pin.OUT)
rojo = Pin(20, Pin.OUT)

# BUZZER
buzzer = PWM(Pin(21))
buzzer.freq(1000)
buzzer.duty_u16(0)

# BOTON PARO / REARME
boton = Pin(22, Pin.IN, Pin.PULL_UP)

# LCD I2C
i2c = I2C(0, sda=Pin(0), scl=Pin(1),
          freq=100000)
LCD_ADDR = 0x27

# ==========================================
# FUNCIONES LCD
# ==========================================

def lcd_nibble(datos):
    base = datos | 0x08
    i2c.writeto(
        LCD_ADDR,
        bytes([base | 0x04])
    )
    i2c.writeto(
        LCD_ADDR,
        bytes([base])
    )

def lcd_enviar(valor, modo=0):
    lcd_nibble((valor & 0xF0) | modo)
    lcd_nibble(((valor << 4) & 0xF0) | modo)

def lcd_comando(valor):
    lcd_enviar(valor)

    if valor in (1, 2):
        sleep_ms(2)

def lcd_iniciar():
    sleep_ms(50)
    lcd_nibble(0x30)
    sleep_ms(5)
    lcd_nibble(0x30)
    sleep_ms(1)
    lcd_nibble(0x30)
    lcd_nibble(0x20)

    lcd_comando(0x28)
    lcd_comando(0x0C)
    lcd_comando(0x06)
    lcd_comando(0x01)

def lcd_texto(texto, fila):
    lcd_comando(0x80 if fila == 0 else 0xC0)

    texto = str(texto)[:16]

    while len(texto) < 16:
        texto += " "

    for letra in texto:
        lcd_enviar(ord(letra), 1)

# ==========================================
# ESTADOS DEL SISTEMA
# ==========================================

INICIO = "INICIO"
ESPERA = "ESPERA"
INGRESO = "INGRESO"
TRANSPORTE = "TRANSPORTE"
CLASIFICACION = "CLASIFICACION"
DESCARGA = "DESCARGA"
PARO = "PARO"
FALLA = "FALLA"

estado = INICIO
inicio_estado = ticks_ms()

# ==========================================
# CONFIGURACION
# ==========================================

DIST_DETECCION = 40
DIST_LIBRE = 45

PASOS_TRANSPORTE = 100
PASOS_DESCARGA = 40

INTERVALO_SERVO = 40
INTERVALO_STEP_US = 8000
INTERVALO_SENSOR = 150
INTERVALO_LCD = 400

MAX_INGRESO = 7000
MAX_TRANSPORTE = 8000
MAX_DESCARGA = 20000

# ==========================================
# VARIABLES
# ==========================================

angulo = 0
posicion = 0
objetivo = 0

umbral = 50
distancia = None
zona_lista = False
errores_sensor = 0

aceptadas = 0
rechazadas = 0
fallas = 0
incompletos = 0

tipo_pieza = ""
motivo_falla = ""

fase_ingreso = 0
inicio_fase = ticks_ms()

ultimo_sensor = ticks_ms()
ultimo_servo = ticks_ms()
ultimo_step = ticks_us()
ultimo_lcd = ticks_ms()
ultimo_reporte = ticks_ms()
ultimo_encoder = ticks_ms()

clk_anterior = clk.value()
sw_anterior = sw.value()

# Variables del boton
lectura_anterior = boton.value()
boton_estable = lectura_anterior
ultimo_rebote = ticks_ms()

rearme_listo = False
inicio_rearme = None

# ==========================================
# FUNCIONES DEL SERVOMOTOR Y A4988
# ==========================================

def mover_servo(grados):
    global angulo

    angulo = max(0, min(90, grados))

    pulso = 500 + angulo * 2000 // 180
    duty = pulso * 65535 // 20000

    servo.duty_u16(duty)

def motor_habilitar(activo):
    enable.value(0 if activo else 1)

def pulso_motor():
    step.on()
    sleep_us(4)
    step.off()

def salidas_seguras():
    motor_habilitar(False)
    step.off()
    mover_servo(0)

# ==========================================
# MEDICION DEL HC-SR04
# ==========================================

def medir_distancia():
    trig.off()
    sleep_us(2)

    trig.on()
    sleep_us(10)
    trig.off()

    try:
        tiempo = time_pulse_us(echo, 1, 30000)

        if tiempo <= 0:
            return None

        cm = tiempo / 58.0

        if cm < 2 or cm > 400:
            return None

        return cm

    except OSError:
        return None

# ==========================================
# INDICADORES
# ==========================================

def actualizar_indicadores():

    verde.value(estado == ESPERA)

    amarillo.value(estado in (
        INGRESO,
        TRANSPORTE,
        CLASIFICACION,
        DESCARGA
    ))

    rojo.value(estado in (PARO, FALLA))

    if estado in (PARO, FALLA):
        buzzer.duty_u16(32768)
    else:
        buzzer.duty_u16(0)

# ==========================================
# CAMBIO DE ESTADO
# ==========================================

def cambiar_estado(nuevo):
    global estado, inicio_estado
    global fase_ingreso, inicio_fase
    global posicion, objetivo, ultimo_step

    if estado == nuevo:
        return

    estado = nuevo
    inicio_estado = ticks_ms()

    print("==============================")
    print("NUEVO ESTADO:", estado)

    if estado == INICIO:
        salidas_seguras()
        print("VERIFICANDO SISTEMA")

    elif estado == ESPERA:
        salidas_seguras()
        print("OPERADOR: COLOQUE PIEZA")

    elif estado == INGRESO:
        fase_ingreso = 0
        inicio_fase = inicio_estado
        print("PIEZA DETECTADA")
        print("ABRIENDO COMPUERTA")

    elif estado == TRANSPORTE:
        posicion = 0
        objetivo = PASOS_TRANSPORTE
        ultimo_step = ticks_us()
        direccion.value(1)
        motor_habilitar(True)
        print("TRANSPORTANDO PIEZA")

    elif estado == CLASIFICACION:
        motor_habilitar(False)
        print("CLASIFICANDO PIEZA")

    elif estado == DESCARGA:
        posicion = 0
        objetivo = PASOS_DESCARGA
        ultimo_step = ticks_us()
        direccion.value(1)
        motor_habilitar(True)
        print("INICIANDO DESCARGA")

    elif estado == PARO:
        salidas_seguras()
        print("PARO ACTIVADO")

    elif estado == FALLA:
        salidas_seguras()
        print("FALLA:", motivo_falla)

    actualizar_indicadores()

# ==========================================
# PARO Y FALLAS
# ==========================================

def activar_paro():
    global incompletos
    global rearme_listo, inicio_rearme

    if estado in (PARO, FALLA):
        return

    if estado in (
        INGRESO, TRANSPORTE,
        CLASIFICACION, DESCARGA
    ):
        incompletos += 1

    rearme_listo = False
    inicio_rearme = None

    cambiar_estado(PARO)

def activar_falla(motivo):
    global fallas, incompletos, motivo_falla
    global rearme_listo, inicio_rearme

    if estado in (PARO, FALLA):
        return

    motivo_falla = motivo
    fallas += 1

    if estado in (
        INGRESO, TRANSPORTE,
        CLASIFICACION, DESCARGA
    ):
        incompletos += 1

    rearme_listo = (boton.value() == 1)
    inicio_rearme = None

    cambiar_estado(FALLA)

# ==========================================
# BOTON PARO / RESTABLECIMIENTO
# ==========================================

def revisar_boton(ahora):
    global lectura_anterior, boton_estable
    global ultimo_rebote, rearme_listo
    global inicio_rearme, zona_lista
    global distancia, errores_sensor
    global motivo_falla

    lectura = boton.value()

    # PARO PRIORITARIO
    if lectura == 0 and estado not in (PARO, FALLA):
        activar_paro()

    # ANTIRREBOTE
    if lectura != lectura_anterior:
        lectura_anterior = lectura
        ultimo_rebote = ahora

    if ticks_diff(ahora, ultimo_rebote) < 40:
        return

    if lectura == boton_estable:
        return

    boton_estable = lectura

    if estado not in (PARO, FALLA):
        return

    # BOTON LIBERADO
    if lectura == 1:

        if not rearme_listo:
            rearme_listo = True
            print("BOTON LIBERADO")
            print("PRESIONE 2 SEG PARA REARME")

        elif inicio_rearme is not None:

            duracion = ticks_diff(
                ahora, inicio_rearme
            )

            inicio_rearme = None

            if duracion >= 2000:

                print("REARME CONFIRMADO")

                errores_sensor = 0
                zona_lista = False
                distancia = None
                motivo_falla = ""

                rearme_listo = False
                cambiar_estado(INICIO)

    # NUEVA PULSACION PARA REARME
    elif lectura == 0 and rearme_listo:
        inicio_rearme = ahora
        print("MANTENGA BOTON 2 SEGUNDOS")

# ==========================================
# MENSAJES LCD PARA EL OPERADOR
# ==========================================

def mostrar_lcd():

    if estado == INICIO:
        lcd_texto("INICIANDO CELDA", 0)
        lcd_texto("ESPERE...", 1)

    elif estado == ESPERA:

        if zona_lista:
            lcd_texto("COLOQUE PIEZA", 0)
        else:
            lcd_texto("LIBERE SENSOR", 0)

        lcd_texto(
            "A:{} R:{} F:{}".format(
                aceptadas,
                rechazadas,
                fallas
            ), 1
        )

    elif estado == INGRESO:
        lcd_texto("INGRESANDO", 0)
        lcd_texto("COMPUERTA ACTIVA", 1)

    elif estado == TRANSPORTE:
        lcd_texto("TRANSPORTANDO", 0)
        lcd_texto(
            "PASOS: {}".format(posicion), 1
        )

    elif estado == CLASIFICACION:
        lcd_texto("CLASIFICANDO", 0)
        lcd_texto("ESPERE...", 1)

    elif estado == DESCARGA:

        if posicion < objetivo:
            lcd_texto("DESCARGANDO", 0)
            lcd_texto(
                "PASOS: {}".format(posicion), 1
            )

        elif (
            distancia is None or
            distancia <= DIST_LIBRE
        ):
            lcd_texto("RETIRE PIEZA", 0)
            lcd_texto("LIBERE SENSOR", 1)

        else:
            lcd_texto("FIN DE DESCARGA", 0)
            lcd_texto("ACTUALIZANDO", 1)

    elif estado == PARO:
        lcd_texto("PARO ACTIVADO", 0)
        lcd_texto("REARME: 2 SEG", 1)

    elif estado == FALLA:
        lcd_texto("FALLA EN CELDA", 0)
        lcd_texto("VER MONITOR", 1)

# ==========================================
# INICIALIZACION
# ==========================================

print("================================")
print("PRACTICA 7 - CELDA MECATRONICA")
print("================================")

salidas_seguras()
actualizar_indicadores()

if LCD_ADDR not in i2c.scan():
    raise RuntimeError(
        "ERROR: LCD NO DETECTADA EN 0x27"
    )

lcd_iniciar()
mostrar_lcd()

print("SISTEMA INICIADO")
print("UMBRAL INICIAL:", umbral, "%")

# ==========================================
# CICLO PRINCIPAL
# ==========================================

while True:

    ahora = ticks_ms()

    # 1. PARO PRIORITARIO
    revisar_boton(ahora)

    # 2. ENCODER
    clk_actual = clk.value()
    sw_actual = sw.value()

    if estado == ESPERA:

        if clk_anterior == 1 and clk_actual == 0:

            if ticks_diff(
                ahora, ultimo_encoder
            ) >= 5:

                ultimo_encoder = ahora

                if dt.value() == 1:
                    umbral = min(100, umbral + 5)
                else:
                    umbral = max(0, umbral - 5)

                print("UMBRAL:", umbral, "%")

        if sw_anterior == 1 and sw_actual == 0:
            umbral = 50
            print("UMBRAL RESTABLECIDO A 50%")

    clk_anterior = clk_actual
    sw_anterior = sw_actual

    # 3. SENSOR HC-SR04
    if ticks_diff(
        ahora, ultimo_sensor
    ) >= INTERVALO_SENSOR:

        ultimo_sensor = ahora

        if estado not in (PARO, FALLA):

            distancia = medir_distancia()

            if distancia is None:
                errores_sensor += 1

                if errores_sensor >= 5:
                    activar_falla("HC-SR04 SIN ECO")

            else:
                errores_sensor = 0

                if estado in (INICIO, ESPERA):
                    if distancia > DIST_LIBRE:
                        zona_lista = True

    # 4. MAQUINA DE ESTADOS
    tiempo = ticks_diff(ahora, inicio_estado)

    if estado == INICIO:

        if tiempo >= 500:
            cambiar_estado(ESPERA)

    elif estado == ESPERA:

        if (
            zona_lista and
            distancia is not None and
            2 <= distancia <= DIST_DETECCION
        ):

            zona_lista = False
            cambiar_estado(INGRESO)

    elif estado == INGRESO:

        if tiempo > MAX_INGRESO:
            activar_falla("TIMEOUT INGRESO")

        elif ticks_diff(
            ahora, ultimo_servo
        ) >= INTERVALO_SERVO:

            ultimo_servo = ahora

            if fase_ingreso == 0:

                mover_servo(angulo + 5)

                if angulo >= 90:
                    fase_ingreso = 1
                    inicio_fase = ahora

            elif fase_ingreso == 1:

                if ticks_diff(
                    ahora, inicio_fase
                ) >= 700:
                    fase_ingreso = 2

            elif fase_ingreso == 2:

                mover_servo(angulo - 5)

                if angulo <= 0:
                    cambiar_estado(TRANSPORTE)

    elif estado == TRANSPORTE:

        if tiempo > MAX_TRANSPORTE:
            activar_falla("TIMEOUT TRANSPORTE")

        elif ticks_diff(
            ticks_us(), ultimo_step
        ) >= INTERVALO_STEP_US:

            ultimo_step = ticks_us()

            pulso_motor()
            posicion += 1

            if posicion >= objetivo:
                motor_habilitar(False)
                cambiar_estado(CLASIFICACION)

    elif estado == CLASIFICACION:

        valor = pot.read_u16() * 100 / 65535

        if valor >= umbral:
            tipo_pieza = "ACEPTADA"
        else:
            tipo_pieza = "RECHAZADA"

        print("RESULTADO:", tipo_pieza)
        print("VALOR ADC:", round(valor, 1), "%")
        print("UMBRAL:", umbral, "%")

        cambiar_estado(DESCARGA)

    elif estado == DESCARGA:

        if tiempo > MAX_DESCARGA:

            activar_falla("TIMEOUT DESCARGA")

        elif posicion < objetivo:

            if ticks_diff(
                ticks_us(), ultimo_step
            ) >= INTERVALO_STEP_US:

                ultimo_step = ticks_us()

                pulso_motor()
                posicion += 1

                if posicion >= objetivo:
                    motor_habilitar(False)
                    print("DESCARGA COMPLETADA")

        else:

            if (
                distancia is not None and
                distancia > DIST_LIBRE
            ):

                if tipo_pieza == "ACEPTADA":
                    aceptadas += 1
                else:
                    rechazadas += 1

                print("============================")
                print("PIEZA:", tipo_pieza)
                print("ACEPTADAS:", aceptadas)
                print("RECHAZADAS:", rechazadas)
                print("FALLAS:", fallas)
                print("INCOMPLETOS:", incompletos)
                print("============================")

                zona_lista = True
                cambiar_estado(ESPERA)

    # 5. ACTUALIZAR INDICADORES
    actualizar_indicadores()

    # 6. PANTALLA LCD
    if ticks_diff(
        ahora, ultimo_lcd
    ) >= INTERVALO_LCD:

        ultimo_lcd = ahora
        mostrar_lcd()

    # 7. REPORTE SERIAL
    if ticks_diff(
        ahora, ultimo_reporte
    ) >= 2000:

        ultimo_reporte = ahora

        print("---------------------------")
        print("ESTADO:", estado)

        if distancia is None:
            print("DISTANCIA: SIN LECTURA")
        else:
            print(
                "DISTANCIA:",
                round(distancia, 1),
                "cm"
            )

        print("UMBRAL:", umbral, "%")
        print("ACEPTADAS:", aceptadas)
        print("RECHAZADAS:", rechazadas)
        print("FALLAS:", fallas)
        print("INCOMPLETOS:", incompletos)
        print("---------------------------")

    sleep_ms(2)