import paho.mqtt.client as mqtt 
import re
from dynio import *
import time
import threading
from statistics import mean
from collections import deque, Counter

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

def set_feed(num):
    global feed_motor
    feed_motor.set_velocity(75)
    feed_motor.torque_enable()
    feed_motor.set_position(int(convert_to_ticks(num)))

def disable_torque():
    global feed_motor
    feed_motor.torque_disable()

def convert_to_ticks(angle):
    # Define the input range
    min_input = 0
    max_input = 90

    # Define the output range
    max_output = 1212
    min_output = max_output - 1024

    # Ensure the angle is within the input range
    angle = max(min(angle, max_input), min_input)

    # Perform the inverse linear transformation
    value = ((angle - min_input) / (max_input - min_input)) * (max_output - min_output) + min_output

    return value

# 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("/jc/feed")

    # Connect to Dynamixels
    global dxl_io, feed_motor
    dxl_io = dxl.DynamixelIO('/dev/ttyS0')
    feed_motor = dxl_io.new_mx64(2, 2)
    feed_motor.get_position()

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

    message = msg.payload.decode()

    pos_num = float(message)  # Extract the number from the message and convert to float
    set_feed(pos_num)

# Define a function to continuously report the log
def continuous_log_reporting():
    global feed_motor
    while True:
        if feed_motor:
            client.publish("/jc/feed/log", (feed_motor.get_position() - 188) * 90 / 1024)
        time.sleep(1)  # Adjust the sleep time as needed

# Start the continuous log reporting in a separate thread
log_thread = threading.Thread(target=continuous_log_reporting)
log_thread.daemon = True
log_thread.start()

   
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()