# shutterbox_osc.py
#
# Sample Webots controller file for driving the
# shutterbox device from OSC network input.
#
# No copyright, 2022, Garth Zeglin.  This file is
# explicitly placed in the public domain.
################################################################
print("loading shutterbox_osc.py...")

# Import the Webots simulator API.
from controller import Robot

# Import standard Python libraries.
import math, threading, queue

# This uses python-osc to communicate with a Max/MSP patch.
#   installation:      pip3 install python-osc
#   source code:       https://github.com/attwad/python-osc
#   pypi description:  https://pypi.org/project/python-osc/
from pythonosc import udp_client
from pythonosc import dispatcher
from pythonosc import osc_server

################################################################
# Define the time step in milliseconds between
# controller updates.
EVENT_LOOP_DT = 20

# Request a proxy object representing the robot to
# control.
robot = Robot()
name = robot.getName()
print(f"shutterbox_osc.py waking up for {name}...")

# Fetch handle for the 'base' joint motor.
motor1 = robot.getDevice('motor1')

################################################################
# Start a background thread running an OSC server listening for messages on an UDP socket.

# Create a thread-safe queue to communicate data to the robot thread.
messages = queue.Queue()

def message(msgaddr, *args):
    """Process messages received via OSC over UDP."""
    print("Controller received message %s: %s" % (msgaddr, args))
    if msgaddr == '/buttonbox/button':
        messages.put(args)

def unknown_message(msgaddr, *args):
    """Default handler for unrecognized OSC messages."""
    print("Simulator received unmapped message %s: %s" % (msgaddr, args))
    
# Initialize the OSC message dispatch system.
dispatch = dispatcher.Dispatcher()
dispatch.map("/buttonbox/*", message)
dispatch.set_default_handler(unknown_message)

# Start and run the server.
simport = 16375 if name == 'left' else 54375
server = osc_server.ThreadingOSCUDPServer(('127.0.0.1', simport), dispatch)
server_thread = threading.Thread(target=server.serve_forever)
server_thread.daemon = True
server_thread.start()
print(f"shutterbox_osc {name} started OSC server on port {simport}")

################################################################
# Run an event loop until the simulation quits,
# indicated by the step function returning -1.
while robot.step(EVENT_LOOP_DT) != -1:

    # Read simulator clock time.
    t = robot.getTime()

    # Check for network messages.
    if not messages.empty():
        # each message is a tuple of the form (button index, status)
        # e.g. (1,1) is button 1 pressed, (1,0) is button 1 released
        msg = messages.get()
        if len(msg) == 2:
            if msg[0] == 1:
                if msg[1] == 1:                
                    motor1.setTorque(-1)
                else:
                    motor1.setTorque(0)
            elif msg[0] == 1:
                if msg[1] == 1:                
                    motor1.setTorque(1)
                else:
                    motor1.setTorque(0)