# Importamos las librerías necesarias
from machine import Pin, PWM, I2C, Timer
import ssd1306
import time

# Configuramos la pantalla OLED
bus_i2c = I2C(-1, scl=Pin(4), sda=Pin(5)) # Creamos un objeto I2C
pantalla_oled = ssd1306.SSD1306_I2C(128, 64, bus_i2c) # Creamos un objeto SSD1306_I2C

# Configuramos los botones
lista_botones = [Pin(i, Pin.IN) for i in [15, 2, 18, 19, 21]] # Creamos una lista de objetos Pin

# Definimos una clase para controlar los servomotores
class ControlServo:
    def __init__(self, pin):
        self.servo = PWM(Pin(pin), freq=47) # Creamos un objeto PWM
        self.lista_rangos = [(0, 180)] # Definimos una lista de rangos por defecto
        self.rango_actual = 0 # Inicializamos el rango actual en 0
        self.en_movimiento = False # Inicializamos el estado de movimiento en False
        self.timer = Timer(-1) # Creamos un objeto Timer para controlar el tiempo de movimiento del servomotor

    def definir_rangos(self, rangos):
        self.lista_rangos = rangos # Actualizamos la lista de rangos
        self.rango_actual = 0 # Reiniciamos el rango actual a 0
        self.en_movimiento = False # Reiniciamos el estado de movimiento a False

    def mover_servo(self):
        if not self.en_movimiento: # Si el servomotor no está en movimiento
            angulo_minimo, angulo_maximo = self.lista_rangos[self.rango_actual] # Obtenemos el rango actual
            for angulo in range(angulo_minimo, angulo_maximo + 1): # Añadimos un bucle para mover el servomotor gradualmente al ángulo medio del rango actual
                ciclo_trabajo = int(angulo / 180 * 120 + 30) # Calculamos el ciclo de trabajo correspondiente al ángulo medio
                self.servo.duty(ciclo_trabajo) # Movemos el servomotor al ángulo medio del rango actual
                time.sleep(0.01) # Añadimos una pequeña pausa para permitir que el servomotor se mueva gradualmente al ángulo medio del rango actual
            self.rango_actual = (self.rango_actual + 1) % len(self.lista_rangos) # Actualizamos el rango actual al siguiente rango en la lista de rangos (o al primer rango si ya estamos en el último rango)
            self.en_movimiento = True # Cambiamos el estado de movimiento a True (el servomotor está en movimiento)
            self.timer.init(period=1000, mode=Timer.ONE_SHOT, callback=lambda t:self.detener_servo()) # Iniciamos un temporizador que detendrá el servomotor después de 1 segundo (1000 milisegundos)
        else: # Si el servomotor está en movimiento
            self.detener_servo() # Detenemos el servomotor

    def detener_servo(self):
        self.servo.duty(0) # Detenemos el servomotor
        self.en_movimiento = False # Cambiamos el estado de movimiento a False (el servomotor no está en movimiento)

# Inicializamos los servomotores y configuramos sus rangos
lista_servomotores = [ControlServo(i) for i in [12, 14, 22, 23]] # Creamos una lista de objetos ControlServo
lista_servomotores[0].definir_rangos([(0, 45), (90, 135), (30, 60), (15, 90)]) 
lista_servomotores[1].definir_rangos([(60, 120), (30, 90), (0, 180), (45, 135)])
lista_servomotores[2].definir_rangos([(0, 30), (75, 120), (15, 75), (90, 135)])
lista_servomotores[3].definir_rangos([(45, 90), (0, 60), (30, 120), (15, 135)])

# Definimos una función para actualizar la pantalla OLED con el ángulo del servomotor
def actualizar_pantalla_oled(angulos_servo):
    pantalla_oled.fill(0) # Limpiamos la pantalla OLED
    for i, angulo in enumerate(angulos_servo): 
        pantalla_oled.fill_rect(0, i*16, int(angulo/180*128), 14, 1) 
        pantalla_oled.text('Servo {}: {}° ({}%)'.format(i+1, angulo, int(angulo/180*100)), 0, i*16)
    pantalla_oled.show() 

# Bucle principal para controlar los servomotores y actualizar la pantalla OLED
while True: # Este bucle se ejecuta indefinidamente
    for i in range(5): # Recorremos los 5 botones
        if lista_botones[i].value() == 1: # Si el botón i está presionado
            if i < 4: # Si el botón presionado es uno de los primeros cuatro
                lista_servomotores[i].mover_servo() # Movemos el servomotor correspondiente
                angulos_servo = [lista_servomotores[i].servo.duty()] # Obtenemos el ángulo actual del servomotor
                actualizar_pantalla_oled(angulos_servo) # Actualizamos la pantalla OLED con el ángulo del servomotor
            else: # Si el botón presionado es el quinto
                for j in range(4): # Recorremos los 4 servomotores
                    lista_servomotores[j].mover_servo() # Movemos cada servomotor
                angulos_servo = [lista_servomotores[j].servo.duty() for j in range(4)] # Obtenemos los ángulos actuales de los servomotores
                actualizar_pantalla_oled(angulos_servo) # Actualizamos la pantalla OLED con los ángulos de los servomotores
            time.sleep(0.1) # Hacemos una pausa de 0.1 segundos para evitar lecturas múltiples si un botón se mantiene presionado y dar tiempo a los servomotores para moverse a sus nuevas posiciones