# suitcase.py
#
# Sample Webots controller file for driving the four-motor suitcase actuator insert.
# The robot also includes two controllable spotlights.
# No copyright, 2021-2023, Garth Zeglin.  This file is explicitly placed in the public domain.

print("loading suitcase.py...")

# Import the Webots simulator API.
from controller import Robot

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

# Load the abstract performance script which will drive this simulated show.
import script

################################################################
# The sample controller is defined as an class which is then instanced as a
# singleton control object.  This is conventional Python practice and also
# simplifies the implementation of the interface which connects this
# code to physical hardware.

class Suitcase(Robot):
    def __init__(self):

        # Initialize the superclass which implements the Robot API.
        super().__init__()

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

        # Fetch handle for the four stepper motors.
        j1 = self.getDevice('motor1')
        j2 = self.getDevice('motor2')
        j3 = self.getDevice('motor3')
        j4 = self.getDevice('motor4')

        # Convenience list of all actuators.
        self.motors = [j1, j2, j3, j4]

        # Connect to the spotlights.
        self.right_spotlight = self.getDevice('right_spotlight')
        self.right_spotlight.set(255)
        self.left_spotlight = self.getDevice('left_spotlight')
        self.left_spotlight.set(255)

        # Current light state used for smoothing changes.
        self.spot_intensity = [0.0, 0.0]

        # Create the abstract controller object implementing the performance.
        self.controller = script.SuitcasePerformance()

        return

    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:
            # Read simulator clock time.
            t = self.getTime()

            # Update the controller object.
            self.controller.poll(t)

            # Apply output values to the simulated hardware.
            for motor, pos in zip(self.motors, self.controller.target):
                motor.setPosition(pos)

            self.spot_intensity[0] += 0.2 * (self.controller.lights['left'] - self.spot_intensity[0])
            self.spot_intensity[1] += 0.2 * (self.controller.lights['right'] - self.spot_intensity[1])
            self.left_spotlight.set(int(255 * self.spot_intensity[0]))
            self.right_spotlight.set(int(255 * self.spot_intensity[1]))


################################################################
# If running directly from Webots as a script, the following will begin execution.
# This will have no effect when this file is imported as a module.
if __name__ == "__main__":
    robot = Suitcase()
    robot.run()