# shutterbox_midi.py
#
# Sample Webots controller file for driving the
# shutterbox device from MIDI 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, queue, platform

# Import the MIDI interface.
# See https://spotlightkid.github.io/python-rtmidi/
# and https://pypi.org/project/python-rtmidi/
import rtmidi

# this should be customized for your particular MIDI controllers
preferred_MIDI_device = 'IAC'
# preferred_MIDI_device = 'MPD218'

################################################################
# 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_midi.py waking up for {name}...")

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

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

# Callback function to receive MIDI messages.
def midi_received(data, unused):
    msg, delta_time = data
    print("MIDI message: ", msg)
    if len(msg) == 3:
        # process NoteOn and NoteOff on channel 9
        if msg[0] == 0x99:  # decimal 153, NoteOn for channel 9
            pad = msg[1] - 35   # pad 1 on the MPD218 is MIDI 36, pad 2 is 37, etc.
            messages.put((pad, 1))
        elif msg[0] == 0x89:  # decimal 137, NoteOff for channel 9
            pad = msg[1] - 35   # pad 1 on the MPD218 is MIDI 36, pad 2 is 37, etc.
            messages.put((pad, 0))

# Initialize the MIDI input system and read the currently available ports.
midi_in = rtmidi.MidiIn()
for idx, midi_name in enumerate(midi_in.get_ports()):
    if preferred_MIDI_device in midi_name:
        print("Found preferred MIDI input device %d: %s" % (idx, midi_name))
        midi_in.open_port(idx)
        midi_in.set_callback(midi_received)
        break
    else:
        print("Ignoring unselected MIDI device: ", midi_name)

if not midi_in.is_port_open():
    if platform.system() == 'Windows':
        print("Virtual MIDI inputs are not currently supported on Windows, see python-rtmidi documentation.")
    else:
        print("Creating virtual MIDI input.")
        midi_in.open_virtual_port(preferred_MIDI_device)
        if not midi_in.is_port_open():
            print("Unable to open MIDI device.")

################################################################
# 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 MIDI 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()
        print("Main loop received", msg)
        if ((name == 'left') and (msg[0] == 1)) or ((name == 'right') and (msg[0] == 3)):
            motor1.setTorque( -1 if msg[1] == 1 else 0)
                
        elif (name == 'left' and msg[0] == 2) or (name == 'right' and msg[0] == 4):
            motor1.setTorque( 1 if msg[1] == 1 else 0)