from machine import Pin, I2C
from time import sleep_ms
import time
from umqtt.simple import MQTTClient
import network
import ssd1306
i2c = I2C(0, scl=Pin(21), sda=Pin(22))
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)
MQTT_BROKER = "broker.hivemq.com"
MQTT_CLIENT_ID = "stepper_12G_13_Martin"
MQTT_TOPIC = "elsys/12g/martin/temp"

A_PLUS_PIN = 26
A_MINUS_PIN = 25
B_PLUS_PIN = 14
B_MINUS_PIN = 12

stepper_pins = [Pin(pin, Pin.OUT) for pin in [A_PLUS_PIN, A_MINUS_PIN, B_PLUS_PIN, B_MINUS_PIN]]


stepper_sequence = [
    [1, 0, 1, 0],  
    [0, 1, 1, 0],
    [0, 1, 0, 0],
    [0, 1, 0, 1],
    [0, 0, 0, 1],
    [1, 0, 0, 1],
    [1, 0, 0, 0],
    [1, 0, 1, 0]
]

speed = 0

def set_pins(values):
    for pin, value in zip(stepper_pins, values):
        pin.value(value)

def control_motor_speed(temp):
    if temp <= 20:
        return 0  
    elif 21 < temp <= 30:
        return 60  
    elif temp > 30:
        return 120  

def calculate_delay(speed_rpm):
    if speed_rpm == 0:
        return 0
    else:
        delay = (130 - speed_rpm)        
        return delay


def connect_to_wifi():
    print("Connecting to Wi-Fi", end="")
    sta_if = network.WLAN(network.STA_IF)
    sta_if.active(True)
    sta_if.connect('Wokwi-GUEST', '')
    while not sta_if.isconnected():
        print(".", end="")
        time.sleep(0.1)
    print(" Connected!")



def on_message(topic, msg):
    global speed
    msg = msg.decode("utf-8")
    print(f"Received message: {msg} on topic {topic}")
    
    speed = control_motor_speed(float(msg))
    oled.fill(0)
    oled.show()
    oled.text('Speed:', 1, 10) 
    oled.text(str(speed), 50, 10)  
    oled.text('RPM', 80, 10) 
    oled.show()
    print("the speed is " + str(speed))
    
client = MQTTClient(MQTT_CLIENT_ID, MQTT_BROKER)

def connect_to_mqtt_broker():
    connect_to_wifi()
    print("Connecting to MQTT server... ", end="")
    client.connect()
    client.set_callback(on_message) 
    print("Connected!")
    client.subscribe(MQTT_TOPIC)
    print("Subscribed to {}".format(MQTT_TOPIC))

connect_to_wifi()
connect_to_mqtt_broker()

try:
    while True:
        client.check_msg()

        delay = calculate_delay(speed)
        
        if delay == 0:
            set_pins([0, 0, 0, 0]) 
        else:
            for step in stepper_sequence:
                set_pins(step)
                sleep_ms(int(delay))

except KeyboardInterrupt:
    client.disconnect()
    print("\nDisconnected from the broker.")