# chest.py
# Copyright, 2024, Garth Zeglin.
# Sample Webots controller file for driving a storage chest with a one-DOF hinge.

print("chest.py waking up.")

# Import standard Python libraries.
import math

# Import the Webots simulator API.
from controller import Robot

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

# Request a proxy object representing the robot to control.
robot = Robot()
robot_name = robot.getName()
print("%s: controller connected." % (robot_name))

# Fetch handles for the motors.
hinge = robot.getDevice('hinge')

# Connect to the radio receiver.
receiver = robot.getDevice('receiver')
receiver.enable(200) # milliseconds of sampling period
message_base_address = "/" + robot_name

# Set time constants for the overall oscillation.
period = 30.0

# run loop to execute a periodic script until the simulation quits.
# If the controller returns -1, the simulator is quitting.
while robot.step(EVENT_LOOP_DT) != -1:
    # Read simulator clock time.
    t = robot.getTime()

    # Oscillate the hinge angle.
    phase = t * (2*math.pi/period)
    hinge.setPosition(0.5 + 0.5*math.sin(phase))

    # Process any radio messages.
    while receiver.getQueueLength() > 0:
        packet = receiver.getString()
        tokens = packet.split()
        if len(tokens) >= 2:
            if tokens[0].startswith(message_base_address):
                if tokens[0] == message_base_address + '/period':
                    period = float(tokens[1])
                    print("%s: set oscillation period to %f" % (robot_name, period))
        # done with packet processing, advance to the next packet
        receiver.nextPacket()