# director.py
#
# Sample Webots controller file for a disembodied robot which coordinates
# activity between sensory robots and actor robots.

# No copyright, 2024, Garth Zeglin.  This file is explicitly placed in the
# public domain.

print("loading director.py...")

################################################################
# standard Python libraries
import math

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

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

################################################################
class Director(Robot):

    def __init__(self):
        super(Director, self).__init__()

        self.robot_name = self.getName()
        print("%s: controller connected." % (self.robot_name))

        # Connect to the radio emitter and receiver.  The radio runs at the same
        # rate as the event loop as it is the main function of this robot.
        self.receiver = self.getDevice('receiver')
        self.emitter  = self.getDevice('emitter')
        self.receiver.enable(EVENT_LOOP_DT)

        # Connect to keyboard input from the GUI.
        self.keyboard = Keyboard()
        self.keyboard.enable(EVENT_LOOP_DT)

        # Initialize top-level script state machines.
        self.state_timer = 2*EVENT_LOOP_DT
        self.state_index = 0
        self.target_timer = 0

        return

    #================================================================
    def poll_receiver(self):
        """Process all available radio messages."""
        while self.receiver.getQueueLength() > 0:
            packet = self.receiver.getString()
            print("%s received: %s" % (self.robot_name, packet))
            tokens = packet.split()
            if len(tokens) >  0:
                if tokens[0] == '/camera/target/dropped':
                    self.emitter.send(b"/left_arm/mode path")
                    self.emitter.send(b"/right_arm/mode path")
                    self.emitter.send(b"/left_wheel/speed -5")
                    self.target_timer = 1000

                elif tokens[0] == '/camera/target':
                    if self.target_timer <= 0:
                        self.emitter.send(b"/left_arm/mode pose")
                        self.emitter.send(b"/right_arm/mode pose")
                        self.emitter.send(b"/left_wheel/speed -15")
                        self.target_timer = 1000

            # done with packet processing, advance to the next packet
            self.receiver.nextPacket()
        # no more data
        return

    #================================================================
    def poll_keyboard(self):
        """Process all available pressed keyboard keys.  Note that the keyboard
        is treated as a state, not events; holding a key will continue to
        generate a reported key, and briefly pressing a key might be lost
        between samples.
        """
        while True:
            key = self.keyboard.getKey()
            if key == -1:
                return
            else:
                code      = key & Keyboard.KEY
                modifiers = "shift" if key & Keyboard.SHIFT else ""
                modifiers += "ctrl" if key & Keyboard.CONTROL else ""
                modifiers += "alt"  if key & Keyboard.ALT else ""

                # convert the integer key number to a lowercase single-character string
                letter = chr(code).lower()
                print("%s: user pressed key %d (%c, %s)" % (self.robot_name, key, letter, modifiers))

                # demonstrate translating a keypress to a network message
                if letter == 'p':
                    self.emitter.send(b"/left_arm/mode pose")

                elif letter == 's':
                    self.emitter.send(b"/left_arm/mode path")

                elif letter == 'r':
                    self.emitter.send(b"/center_light/color 255 0 0")
                    print("%s: sending lighting command" % (self.robot_name))

                elif letter == 'b':
                    self.emitter.send(b"/center_light/color 0 0 0")
                    print("%s: sending lighting command" % (self.robot_name))

                elif letter == 'f':
                    self.emitter.send(b"/left_wheel/speed -20")
                    print("%s: sending speed command" % (self.robot_name))

                elif letter == 'c':
                    self.emitter.send(b"/chest/period 5")
                    print("%s: sending period command" % (self.robot_name))

    #================================================================
    def poll_scene_sequence(self):
        """State machine update function to walk through a series of scene phases."""

        # Update the state timers
        self.state_timer -= EVENT_LOOP_DT
        self.target_timer -= EVENT_LOOP_DT

        # If the timer has elapsed, reset the timer and update the state.
        if self.state_timer < 0:
            self.state_timer += 5000

            # here is where scene changes could be programmed

    #================================================================
    def run(self):

        # Run loop to execute a periodic script until the simulation quits.
        # If the controller returns -1, the simulator is quitting.
        while self.step(EVENT_LOOP_DT) != -1:

            # Poll the simulated radio receiver.
            self.poll_receiver()

            # Poll user input.
            self.poll_keyboard()

            # Read simulator clock time.
            t = self.getTime()

            # Update scene state machine.
            self.poll_scene_sequence()

################################################################

controller = Director()
controller.run()