#!/usr/bin/env python3

# Enable basic compatibility features to work with either Python 2 or 3.
from __future__ import print_function, absolute_import, unicode_literals

# Standard library modules.
import os, sys, time, argparse

# This requires a pySerial installation.
from serial.serialutil import SerialException

# Configure load path to include the parent folder so the course library can be found in a parallel folder.
sys.path.insert(0, os.path.dirname(os.path.abspath(os.path.dirname(__file__))))

# Import modules from the course library.
import ase.logging
import ase.events
import ase.SerialTransport
import ase.MiniMaestroProtocol

#================================================================
[docs]def animation_sequence(servos, numservos, step, logger):
    """Produce an animation motion sequence on a set of servos by issuing motion
    commands to the servo interface and performing delays using
    ase.events.sleep().  This is a good starting point for customizing the
    script.

    :param servos: handle to the Mini Maestro transport object
    :param numservos: number of servos to assume are connected
    :param step: flag to indicate frame-by-frame stepping
    :param logger: logging object for debugging and status output
    """

    # Define a convenience function to separate each frame.
    def sleep_or_wait(duration):
        if step:
            print("Press Enter to continue...")
            ase.events.wait_for_keyboard_input([servos])
        else:
            ase.events.sleep(duration, [servos])

    # Extract the MiniMaestroProtocol object from the transport for convenience.
    protocol = servos.protocol

    # Endless loop to run the sequence in a loop.
    while True:
        # Send motion commands to the given number of channels.

        # The list notation '[2.0]*numservos' duplicates the 2.0 to make a list
        # of position values the right length (one per channel).

        protocol.send_multiple_speeds(0, [2.0]*numservos)
        protocol.send_multiple_accelerations(0, [5.0]*numservos)
        protocol.send_multiple_targets(0, [1.0]*numservos)

        # Sleep for one second while processing data replies from the interface.
        sleep_or_wait(1.0)

        # Send position commands to individual servos.
        for i in range(numservos):
            protocol.send_target(i, 1.5)
            sleep_or_wait(0.2)

        # Move all back to start.
        protocol.send_multiple_targets(0, [1.0]*numservos)
        sleep_or_wait(1.0)

        # Set unlimited speed and acceleration and move to the far position.
        protocol.send_multiple_speeds(0, [0.0]*numservos)
        protocol.send_multiple_accelerations(0, [0.0]*numservos)
        protocol.send_multiple_targets(0, [2.0]*numservos)
        sleep_or_wait(1.0)

#================================================================
[docs]def main(argv):
    """Main program entry point.

    :param argv: list of command line arguments as individual strings
    """

    # Initialize the command parser.
    parser = argparse.ArgumentParser( description = """Test animation using a Mini Maestro servo interface board.""")
    ase.logging.add_logging_arguments(parser)
    parser.add_argument('--list', action='store_true', help='Print list of available serial ports.')
    parser.add_argument('--step', action='store_true', help='Step frame by frame through sequence.')
    parser.add_argument('--servos', type=int, default=6, help='Specify the number of servo channels.  Default: 6.')

    default_serial_port = '/dev/tty.usbmodem00146911'
    parser.add_argument( '--maestro', default=default_serial_port,
                         help='Specify the name of the Mini Maestro Command Port device. Default: %s.' % default_serial_port)
    args = parser.parse_args(argv)

    # Configure a typical logging setup.
    logger, handlers = ase.logging.create_script_logger('animation', args)

    # Print the list of serial ports to console if requested.
    if args.list:
        ase.SerialTransport.print_serial_port_list()

    # Create an object to manage the serial port.
    servos = ase.SerialTransport.SerialTransport(ase.MiniMaestroProtocol.MiniMaestroProtocol(), args.maestro, 115200)
        
    # Connect to the board.
    logger.info("Connecting to Mini Maestro on %s.", args.maestro)
    try:
        servos.open()
    except SerialException as exp:
        logger.error("Unable to open port: %r", exp)
        logger.info("You might try the --list argument to see the available serial ports.\nExiting.")
        sys.exit(1);

    # Now run the event loop until it exits or control-C is pressed.
    try:
        animation_sequence(servos, args.servos, args.step, logger)

    except KeyboardInterrupt:
        logger.info("User interrupted operation.")

    # Shut down the connection.
    servos.close()

#================================================================
# The following section is run when this is loaded as a script.
if __name__ == "__main__":

    # Call the main function with most of the command line arguments.
    # This could be customized by pre-defining particular arguments:
    constant_args = []
    # constant_args = ['--maestro', '/dev/tty.usbmodem00146911']

    main(constant_args + sys.argv[1:])