# legbox.py
# Copyright, 2026, Garth Zeglin.
# Demo of a box with two wide front-and-back legs.

print("legbox.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.
front_hip  = robot.getDevice('front_hip')
front_knee = robot.getDevice('front_knee')

rear_hip = robot.getDevice('rear_hip')
rear_knee = robot.getDevice('rear_knee')

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

# 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 angles.  Each hinge swings 180 degrees, but out of phase.
    # The phase variable advances by 360 degrees each period.
    phase = t * (2*math.pi/period)

    knee_bend = 0.7
    front_hip.setPosition(-0.5*knee_bend*math.pi * max(0, math.sin(phase)))
    front_knee.setPosition(knee_bend*math.pi * max(0, math.sin(phase)))

    rear_hip.setPosition(-0.5*knee_bend*math.pi * max(0, math.sin(phase + math.pi)))
    rear_knee.setPosition(knee_bend*math.pi * max(0, math.sin(phase + math.pi)))