import board
import time
from ideaboard import IdeaBoard

import adafruit_hcsr04
from adafruit_onewire.bus import OneWireBus
import adafruit_ds18x20


# Instancias de objetos
ib = IdeaBoard() # Instancia de placa microcontroladora

ow_bus = OneWireBus(board.IO33)  # Inicializa OneWire bus asociado al sensor de temperatura
devices = ow_bus.scan()

SW = ib.DigitalIn(board.IO27) # Instancia el Switch
POT = ib.AnalogIn(board.IO32) # Instancia el potenciomentro

ULTRA = adafruit_hcsr04.HCSR04(trigger_pin=board.IO4, echo_pin=board.IO5) #Instancia el sensor Ultrasonico
SERVO1 = ib.Servo(board.IO18)  # Instancia el Servo
ds18b20 = adafruit_ds18x20.DS18X20(ow_bus, devices[0]) #instancia el sensor de temperatura


motor_dc = ib.motor_2 # Instancia de Motor DC 

TEMP_MAX = 35.0  # Umbral de sobrecalentamiento en grados Celsius

#-------------- Funciones del sistema----------

def medir_distancia(): # Función 1, lee la distancia del sensor ultrasónico
    
    try:
        distancia = ULTRA.distance
        print(f"Distancia medida: {distancia:.2f} cm")
        return distancia
    except RuntimeError:
        print("Error de lectura en distancia, reintentando...")
        return None


def seleccionar_objeto(distancia): # Función 2 mueve el según la altura del paquete
    
    if distancia is None: 
        return
    if abs(distancia - 10.0) < 0.5:
        SERVO1.angle = 45   # Desviar a la derecha
        print("Paquete pequeño, desviado a la derecha")

    elif abs(distancia - 15.0) < 0.5:
        SERVO1.angle = 135  # Desviar a la izquierda
        print("Paquete mediano, desviado a la izquierda")

    elif abs(distancia - 20.0) < 0.5:
        SERVO1.angle = 90   # Dejar pasar, no se desvía a ningún lado
        print("Paquete grande, dejando pasar...")

    else:
        SERVO1.angle = 90   # Por defecto el brazo estará centrado (FALSE)

def controlar_motor_y_temp():  # Función 3, controla la velocidad del motor según el potenciómetro y verifica temperatura
    
    try:
        temp = ds18b20.temperature
        print(f"Temperatura: {temp:.2f} °C")
    except RuntimeError as e:
        print(f"Error de lectura de temperatura: {e}")
        return  

    # Control del motor
    if SW.value:  # Si el switch está presionado
        pot_val = POT.value
        velocidad = ib.map_range(pot_val, 2819, 62139, 0, 1)
        motor_dc.throttle = velocidad
        print(f"Motor encendido. Velocidad: {velocidad:.2f}")
    else:
        motor_dc.throttle = 0
        print("Motor apagado (switch no presionado)")

    # Control de temperatura y neopixel
    if temp > TEMP_MAX:
        ib.pixel = (255, 0, 0)  # Rojo
        print("Motor caliente")
    else:
        ib.pixel = (0, 255, 0)  # Verde
        print("Temperatura normal")

# Llamada a las 3 funciones creadas
while True:
    dist = medir_distancia() # ultrasónico para medir altura de objetos.
    seleccionar_objeto(dist) # Para mover el brazo del servomotor y hacer selección según altura detectada.
    controlar_motor_y_temp() # Para controlar el encendido con el Switch , velocidad de banda con potenciómetro y temp por el Neopixel.
    time.sleep(0.5)