from machine import Pin, ADC, PWM, I2C
import ssd1306
import time

i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400000)
oled = ssd1306.SSD1306_I2C(128, 64, i2c)
joy_x = ADC(26)      
joy_y = ADC(27)      
boton_modo = Pin(22, Pin.IN, Pin.PULL_UP)      
boton_activar = Pin(15, Pin.IN, Pin.PULL_UP)  
boton_rutina = Pin(8, Pin.IN, Pin.PULL_UP) 
servo1 = PWM(Pin(2))
servo2 = PWM(Pin(3))
servo3 = PWM(Pin(4))
servo4 = PWM(Pin(5))
servo5 = PWM(Pin(6))
servo6 = PWM(Pin(7))


servos = [servo1,servo2,servo3,servo4,servo5,servo6]
for servo in servos:
    servo.freq(50)
led_rojo = Pin(11, Pin.OUT)    
led_verde = Pin(13, Pin.OUT)   
led_ambar = Pin(12, Pin.OUT)    
trig = Pin(16, Pin.OUT)
echo = Pin(17, Pin.IN)
def angulo_a_duty(angulo):

    us = 500 + (angulo / 180) * 2000

    return int(us / 20000 * 65535)

def mover_servo(servo, angulo):

    servo.duty_u16(
        angulo_a_duty(angulo)
    )

def medir_distancia():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()
    while echo.value() == 0:
        inicio = time.ticks_us()
    while echo.value() == 1:
        fin = time.ticks_us()
    duracion = time.ticks_diff(fin, inicio)
    distancia = (duracion * 0.0343) / 2
    return distancia
DEADZONE = 8000
CENTRO = 32768
VELOCIDAD = 1.5
angulos = [90,90, 90, 90, 90, 90]
for i in range(6):

    mover_servo(
        servos[i],
        angulos[i]
    )
def pantalla_inicio():

    oled.fill(0)

    oled.text(
        "ULTRAKILL",
        20,
        20
    )
    oled.show()
    time.sleep(1)
pantalla_inicio()
modo = 1
ultimo_boton_modo = 1
def rutina_automatica():

    secuencia = [
        (0, 140),
        (1, 40),
        (2, 150),
        (3, 30),
        (4, 120),
        (5, 60)
    ]

    for servo, destino in secuencia:

        angulos[servo] = destino
        mover_servo(servos[servo], angulos[servo])
        time.sleep(0.6)

        angulos[servo] = 90
        mover_servo(servos[servo], angulos[servo])
        time.sleep(0.4)

while True:
    if boton_activar.value() == 0:
        robot_activo = True
        led_rojo.on()
        led_verde.off()
    else:
        robot_activo = False
        led_rojo.off()
        led_verde.on()
    estado_modo = boton_modo.value()
    if ultimo_boton_modo == 1 and estado_modo == 0:
        modo += 1
        if modo > 3:
            modo = 1
        time.sleep_ms(250)
    ultimo_boton_modo = estado_modo
    if boton_rutina.value() == 0:

        rutina_automatica()

        while boton_rutina.value() == 0:
            time.sleep_ms(20)
    if robot_activo:
        x = joy_x.read_u16()
        y = joy_y.read_u16()
        if modo == 1:
            servo_x = 0
            servo_y = 1
        elif modo == 2:
            servo_x = 2
            servo_y = 3
        else:
            servo_x = 4
            servo_y = 5
        if x > CENTRO + DEADZONE:
            angulos[servo_x] += VELOCIDAD
        elif x < CENTRO - DEADZONE:
            angulos[servo_x] -= VELOCIDAD
        angulos[servo_x] = max(
            0,
            min(
                180,
                angulos[servo_x]
            )
        )
        if y > CENTRO + DEADZONE:
            angulos[servo_y] += VELOCIDAD
        elif y < CENTRO - DEADZONE:
            angulos[servo_y] -= VELOCIDAD
        angulos[servo_y] = max(
            0,
            min(
                180,
                angulos[servo_y]
            )
        )
        for i in range(6):
            mover_servo(
                servos[i],
                angulos[i]
            )
    distancia = medir_distancia()
    if distancia < 10:

        led_ambar.on()
    else:

        led_ambar.off()
    oled.fill(0)
    oled.text(
        "ULTRAKILL",
        20,
        0
    )
    if robot_activo:
        oled.text(
            "Estado:ON",
            0,
            12
        )
    else:

        oled.text(
            "Estado:OFF",
            0,
            12
        )
    oled.text(
        "Modo:{}".format(modo),
        80,
        12
    )
    oled.text(
        "S1:{} S2:{}".format(
            int(angulos[0]),
            int(angulos[1])
        ),
        0,
        26
    )
    oled.text(
        "S3:{} S4:{}".format(
            int(angulos[2]),
            int(angulos[3])
        ),
        0,
        38
    )
    oled.text(
        "S5:{} S6:{}".format(
            int(angulos[4]),
            int(angulos[5])
        ),
        0,
        50
    )
    oled.show()
    time.sleep(0.02)