#!/usr/bin/env python
"""\
valve_sequence.py

Play back a sequence of pneumatic valve activations using a serial port
connection to an Arduino running ValveControl.

Copyright (c) 2015-2017, Garth Zeglin.  All rights reserved. Licensed under the
terms of the BSD 3-clause license.

"""
################################################################
# Import standard Python 2.7 modules.
from __future__ import print_function
import os, sys, time, argparse

# This module requires a pySerial installation.
#  Package details: https://pypi.python.org/pypi/pyserial,
#  Documentation: http://pythonhosted.org/pyserial/
import serial

################################################################
def set_flow( port, flow, extend=1, retract=2, pause=None, verbose=False):
    """Issue an open-loop set of flow commands for a valve pair attached to an actuator.
    Returns immediately or after an optional delay.  No return value.

    :param port: serial port stream handle
    :param flow: net flow from -100 full retraction rate to 100 full extension rate
    :param extend: channel number for extension valve pair
    :param retract: channel number for retract valve pair
    :param pause: time in seconds to delay after issuing command
    :param verbose: flag to enable printed debugging output
    :return: None
    """


    if flow > 0:
        ext_command  = "speed %d %d" % (extend, flow)
        ret_command = "speed %d %d" % (retract, -100)
    elif flow == 0:
        ext_command  = "speed %d %d" % (extend, 0)
        ret_command = "speed %d %d" % (retract, 0)
    else:
        ext_command  = "speed %d %d" % (extend, -100)
        ret_command = "speed %d %d" % (retract, -flow)

    if verbose:
        print ("Issuing %s, %s" % (ext_command, ret_command))

    port.write("%s\n%s\n" % (ext_command, ret_command))

    if pause is not None:
        time.sleep(pause)

    # ignore any sensor feedback
    port.flushInput()

    return

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

    # Initialize the command parser.
    parser = argparse.ArgumentParser( description = """Scripted sequence of pneumatic valve activations using the ValveControl controller on an Arduino.""")
    parser.add_argument( '-v', '--verbose', action='store_true', help='Enable more detailed output.' )
    parser.add_argument( '--debug', action='store_true', help='Enable debugging output.' )
    parser.add_argument( '-p', '--port', default='/dev/tty.usbmodem1411', help='Specify the name of the Arduino serial port device (default is /dev/tty.usbmodem1411).')

    # Parse the command line, returning a Namespace.
    args = parser.parse_args()

    # Open the serial port, which should also reset the Arduino
    print("Connecting to Arduino.")
    port = serial.Serial( args.port, 115200, timeout=5 )
    if args.verbose:
        print("Opened serial port named", port.name)

    print("Sleeping briefly while Arduino boots...")
    time.sleep(2.0)

    # throw away any extraneous input
    print("Flushing Arduino input...")
    port.flushInput()

    # Begin the motion sequence.  This may be safely interrupted by the user pressing Control-C.
    try:
        print("Beginning movement sequence.")
        set_flow(port,   50, pause=2.0, verbose=args.verbose)
        set_flow(port,    0, pause=2.0, verbose=args.verbose)
        set_flow(port,  -50, pause=2.0, verbose=args.verbose)
        set_flow(port,    0, pause=2.0, verbose=args.verbose)
        set_flow(port,  100, pause=2.0, verbose=args.verbose)
        set_flow(port,    0, pause=2.0, verbose=args.verbose)
        set_flow(port, -100, pause=2.0, verbose=args.verbose)
        print("Movement complete.")

    except KeyboardInterrupt:
        print("User interrupted motion.")

    # Close the serial port connection.
    port.write("stop\n")
    port.close()