# script.py
# Sample abstract performance script for operating the suitcase and lighting hardware on either
# the Webots simulation or the physical hardware.
# No copyright, 2023, Garth Zeglin.  This file is explicitly placed in the public domain.

print("loading script.py...")

# Standard Python imports.
import math, random

# Define the abstract performance interface.
from performance import Performance

#================================================================
# Define a set of named poses.  The actuators are numbered in the following order:
#  1  2
#  3  4

poses = { 'home' : [0, 0, 0, 0],
          'box'  : [   math.pi/2,   math.pi/2,   math.pi/2,    math.pi/2],
          '1'    : [   math.pi/2,           0,           0,            0],
          '2'    : [           0,   math.pi/2,           0,            0],
          '3'    : [           0,           0,   math.pi/2,            0],
          '4'    : [           0,           0,           0,    math.pi/2],
          }

sequence = ['home', '1', 'home', '2', 'home', '4', 'home', '3', 'home', 'box']

# generator function to produce infinite sequence of joint vectors
def sequence_generator():
    while True:
        for name in sequence:
            yield poses[name]

#================================================================

# Define an abstract performance controller which can run either the Webots simulation or a physical machine.
class Impeller_Test_1_Performance(Performance):
    def __init__(self):

        # Initialize the superclass which implements the abstract robot interface.
        super().__init__()

        # Initialize the controller state machine
        self.last_t = 0.0
        self.event_timer = 0.0
        self.state_index = 'rest'

        # Initialize the sequence
        self.generator = sequence_generator()

    # Entry point for periodic updates.
    def poll(self, t):
        dt = t - self.last_t
        self.last_t = t

        # Change the target velocity in a cycle with a two-second period.
        self.event_timer -= dt
        if self.event_timer < 0:
            if self.state_index == 'left':
                self.set_spotlight('right', 1.0)
                self.set_spotlight('left', 0.0)
                self.state_index = 'right'
                self.event_timer += 1.5
                pose = next(self.generator)
                for j, pos in enumerate(pose):
                    self.set_motor_target(j, pos)

            else:
                self.state_index = 'left'
                self.set_spotlight('right', 0.0)
                self.set_spotlight('left', 1.0)
                self.event_timer += 1.5
                pose = next(self.generator)
                for j, pos in enumerate(pose):
                    self.set_motor_target(j, pos)

            print(f"Switched to state {self.state_index}")