import paho.mqtt.client as mqtt
import re
from dynio import *
import time

# State variable to check if HOME is received before POS
home_received = False
home = 0
dxl_io = []
motor = None

# Functions to run when the corresponding command is received
def home():
    global motor, home
    motor.write_control_table('Torque_Limit', 200)
    motor.set_velocity_mode()
    print('velocity mode set')
    motor.torque_enable()
    print('torque enabled')
    motor.set_velocity(300)
    print('velocity set')
    time.sleep(1)
    while motor.read_control_table('Present_Speed') > 0:
        pass
    motor.torque_disable()
    home = motor.get_position()
    motor.write_control_table('Multi_Turn_Offset', -1 * home)
    motor.set_extended_position_mode()
    motor.torque_enable()

def set_pos(num):
    global motor
    motor.set_position(num)
    time.sleep(0.2)
    while motor.read_control_table('Present_Speed') > 0:
        pass

def disable_torque():
    global motor, home_received
    motor.torque_disable()
    home_received = False


# The callback for when the client receives a CONNACK response from the server.
def on_connect(client, userdata, flags, rc):
    print(f"Connected with result code {str(rc)}")

    # Subscribing in on_connect() means that if we lose the connection and
    # reconnect then subscriptions will be renewed.
    client.subscribe("/meyer/#")

    # Connect to Dynamixels
    global dxl_io, motor
    dxl_io = dxl.DynamixelIO('/dev/ttyACM0')
    motor = dxl_io.new_mx106(1,1)
    motor.get_position()
    motor.write_control_table('Torque_Limit', 200)


# The callback for when a PUBLISH message is received from the server.
def on_message(client, userdata, msg):
    global home_received, motor

    message = msg.payload.decode()
    if msg.topic != "/meyer/":
        return

    if message == "HOME":
        home()
        home_received = True
        time.sleep(0.5)
        set_pos(int(-250*53.248))
        client.publish("/meyer/pos", -250.0)
    elif message == "DISABLE":
        disable_torque()
    elif re.match(r"^POS -?\d+$", message):  # This matches POS followed by a number
        pos_num = int(message.split()[1])  # Extract the number from the message
        pos_num *= 5.3248

        if pos_num < -25000 or pos_num > 0:  # Check if the number is out of range
            client.publish("/meyer/error", "Error: Number out of range")
        elif not home_received:  # Check if HOME was received before POS
            client.publish("/meyer/error", "Error: HOME command has to be run before POS")
        else:
            set_pos(int(pos_num))
            client.publish("/meyer/pos", round(pos_num / 53.248,1))
    else:
        client.publish("/meyer/error", "Error: Unknown command")
    

client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message

# Connect to the broker
client.connect("192.168.1.1", 1883, 60)

# Blocking call that processes network traffic, dispatches callbacks and
# handles reconnecting.
client.loop_forever()