import socket
import struct
import threading
import time
import signal

def tcp_send_command_thread(stop_event):
    # Configure Socket
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        # Connect to motor
        s.connect(('192.168.1.102', 1000))

        # Compose String to send to the drive
        st = '"<control pos=\"15000\" frequency=\"20\" torque=\"200\" mode=\"135\" offset=\"0\" phase=\"0\" />"'

        s.sendall(st.encode('ascii')) # send XML command  
        
        # The main thread can continue executing other tasks
        while not stop_event.is_set():
            time.sleep(1)
    
def receive_udp_data(udp_port, stop_event):
    # Configure Socket
    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
        s.bind(('', udp_port))

        while not stop_event.is_set():
            data, addr = s.recvfrom(132) # buffer size is 132 bytes
            int_list = list(struct.unpack("<33i", data)) # interprete 33 int32 numbers
            
            # Print the list
            print('time:' + str(int_list[0]) + ' position:' + str(int_list[1]) + ' speed:' + str(int_list[2]))

def keyboard_interrupt_handler(signal, frame):
    print("Keyboard interrupt received. Stopping the program.")
    # Set the stop event to signal the thread to exit
    stop_event.set()

    # Exit the program
    exit(0)

if __name__ == '__main__':

    stop_event = threading.Event()  # create an event object

    # create a thread that receives on UDP port 1001    
    udp_receive_thread = threading.Thread(target=receive_udp_data, args=(1001, stop_event))
    udp_receive_thread.start()

    # create a thread that can send commands to the motor    
    tcp_send_command_thread = threading.Thread(target=tcp_send_command_thread, args=(stop_event,))
    tcp_send_command_thread.start()

    # Install the keyboard interrupt handler
    signal.signal(signal.SIGINT, keyboard_interrupt_handler)

    # The main thread can continue executing other tasks
    while True:
        # Do other stuff here
        time.sleep(1)