# spotlight.py
# Copyright, 2024, Garth Zeglin.
# Sample Webots controller file for driving a fixed spotlight with controllable color.

print("spotlight.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 = 50

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

# Connect to the spotlight.
spotlight = robot.getDevice('lamp')

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

# Use the customData field to configure the operating mode.
mode = robot.getCustomData()

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

    if mode == "color_cycle":
        # Oscillate the color.
        phase = t * (2*math.pi/5.0)
        red   = min(max(int(255*(0.5 + 0.5*math.sin(phase))), 0), 255)
        green = 128
        blue  = min(max(int(255*(0.5 + 0.5*math.sin(1.0 + 1.2*phase))), 0), 255)
        color = blue | (green<<8) | (red << 16)
        # print("color value: 0x%x" % (color))
        spotlight.set(color)

    # Process any radio messages.
    while receiver.getQueueLength() > 0:
        packet = receiver.getString()
        # print("%s Receiver: %s" % (robot_name, packet))
        tokens = packet.split()
        if len(tokens) >= 4:
            if tokens[0].startswith(message_base_address):
                if tokens[0] == message_base_address + '/color':
                    red   = int(tokens[1])
                    green = int(tokens[2])
                    blue  = int(tokens[3])
                    spotlight.set(blue | (green<< 8) | (red << 16))
                    print("%s: set light color to %d, %d, %d" % (robot_name, red, green, blue))

        # done with packet processing, advance to the next packet
        receiver.nextPacket()