# shutterbox.py
#
# Sample Webots controller file for driving the
# shutterbox device.
#
# No copyright, 2022, Garth Zeglin.  This file is
# explicitly placed in the public domain.

print("loading shutterbox.py...")

# Import the Webots simulator API.
from controller import Robot
from controller import Keyboard

# Import the standard Python math library.
import math

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

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

# Enable computer keyboard input for user control.
keyboard = Keyboard()
keyboard.enable(EVENT_LOOP_DT)

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

    # Read any computer keyboard keypresses.  Returns -1 or an integer keycode while a key is held down.
    # This is debounced to detect only changes.
    key = keyboard.getKey()
    if key != -1:
        # convert the integer key number to a lowercase single-character string
        letter = chr(key).lower()

        # Apply motor torques based on keypresses and robot identity.
        if name == 'left':
            if letter == 'a':
                motor1.setTorque(-1.0)
            elif letter == 'd':
                motor1.setTorque(1.0)
        else:
            if letter == 'j':
                motor1.setTorque(-1.0)
            elif letter == 'l':
                motor1.setTorque(1.0)
    else:
        motor1.setTorque(0.0)