import time
import uasyncio as asyncio
from machine import Pin, I2C, lightsleep
from my_lib import ControlStepper, ButtonHandler, PrintLED
from ssd1306 import SSD1306_I2C

# Wait for USB to become ready
time.sleep(0.1)

print("Hello, Pi Pico!")

# Инициализация пинов LED
led_blue = Pin(15, Pin.OUT)
led_green = Pin(2, Pin.OUT)

# Инициализация пинов кнопок
btn_blue = Pin(27, Pin.IN)
btn_green = Pin(26, Pin.IN)

# Инициализируем экземляры классов ButtonHandler для каждой кнопки
button_blue = ButtonHandler(btn_blue)
button_green = ButtonHandler(btn_green)

# Инициализация OLED и интерфейса I2C
WIDTH = 128
HEIGHT = 64
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=200000)
oled = SSD1306_I2C(WIDTH, HEIGHT, i2c)

# Инициализируем экземляр класса PrintLED для OLED дисплея
oled_info = PrintLED(oled)

# Флаг необходимости логирования шага двигателей
log = True

# Create an instance of ControlStepper with the pin numbers
# Создаём экземпляр класса(объект) с заданной распиновкой подключения мотора №1 (Синий)
stepper_blue = ControlStepper(Am_pin=13, Ap_pin=12, Bm_pin=10, Bp_pin=11)

# Создаём экземпляр класса(объект) с заданной распиновкой подключения мотора №2 (Зелёный)
stepper_green = ControlStepper(Am_pin=9, Ap_pin=8, Bm_pin=6, Bp_pin=7)

async def main():
    oled_info.print("Push any button!", Print=log)

    while True:
        if button_blue.is_pressed():
            led_blue.toggle()
            #time.sleep(0.5)
            if led_blue.value() == 1:
                oled_info.print("MOTOR BLU is ON", Print=log)
                stepper_blue.start_continuous()
            else:
                oled_info.print("MOTOR BLU is OFF", Print=log)
                stepper_blue.stop()
        await asyncio.sleep_ms(10)  # Небольшая задержка для снижения нагрузки на CPU

        if button_green.is_pressed():
            led_green.toggle()
            #time.sleep(0.5)
            if led_green.value() == 1:
                oled_info.print("MOTOR GRE is ON", Print=log)
                stepper_green.start_continuous()
            else:
                oled_info.print("MOTOR GRE is OFF", Print=log)
                stepper_green.stop()
        await asyncio.sleep_ms(10)  # Небольшая задержка для снижения нагрузки на CPU

# Запуск асинхронного цикла
asyncio.run(main())



# oled_info.print("Push any button!", Print=log)

# while True:   
#     if button_blue.is_pressed():
#         led_blue.toggle()
#         time.sleep(0.5)
#         if led_blue.value() == 1:
#             oled_info.print("LED BLUE is ON", Print=log)
#             # stepper_blue.step(99999999)
#         else:
#             oled_info.print("LED BLUE is OFF", Print=log)
#             stepper_blue.stop()





# # Call the step method on the instance
# control_stepper1.step(steps=2, Print=True)

# # Call the step method on the instance
# control_stepper2.step(steps=2)

# # Почешемся 3 секунды
# time.sleep(3)
# # И ещё довернём на 5 шагов
# control_stepper1.step(5, mode=1, Print=log)

# control_stepper2.step(11, mode=1, Print=log)

# # Ждём ещё 5 секунды
# time.sleep(3)

# control_stepper1.step(mode=1, Print=log)


# time.sleep(2)
# control_stepper2.step(23, mode=1, Print=True)