#!/usr/bin/env python
"""\
four_joint_example.py : example command-line program to communicate with two Arduinos over serial ports and execute timed logic.

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

from __future__ import print_function
import os, sys, time, argparse, select

from ValveControlProtocol import ValveControlProtocol
from SensorProtocol import SensorProtocol

################################################################
def polling_wait(arduinos, duration=0.0):
    """Poll all Arduinos and process pending data for a given interval.

    :param arduinos: list of Arduino protocol objects
    :param duration: time in seconds to poll before returning (default to zero)
    """
    last_time = time.time()
    pending = True

    while pending or duration > 0.0:
        # Check which Arduinos have data available, using select with a short timeout.
        ready = select.select(arduinos,[], [], max(duration, 0.0))
        pending = False
        
        # Process messages on each Arduino port with available data.  If any message
        # is received, reflag pending to ensure checking again.
        for arduino in ready[0]:
            arduino.wait_for_message()
            pending = True
          
        # Recalculate the maximum timeout.
        if duration > 0.0:
            now = time.time()
            duration -= (now - last_time) # subtract elapsed time
            last_time = now

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

    # Initialize the command parser.
    parser = argparse.ArgumentParser( description = """Example command line program to interface with an Arduino sketch.""")
    parser.add_argument( '-v', '--verbose', action='store_true', help='Enable more detailed output.' )
    parser.add_argument( '--debug', action='store_true', help='Enable debugging output.' )

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

    # Extract specific command line options into a keyword argument dictionary.
    kwargs = { 'debug' : args.debug, 'verbose' : args.verbose }
    
    # Create protocol objects to manage the serial ports.
    valve1 = ValveControlProtocol(port = "/dev/tty.usbmodem141431", **kwargs)
    valve2 = ValveControlProtocol(port = "/dev/tty.usbmodem141441", **kwargs)
    sensors = SensorProtocol(     port = "/dev/tty.usbmodem141421", **kwargs)
    arduinos = [valve1, valve2, sensors]
    
    # Connect to the Arduinos, which will also reset them.
    print("Connecting to Arduinos.")
    
    for arduino in arduinos:
        arduino.open_serial_port()

    # issue a few test commands
    time.sleep(2)

    # Begin an animation sequence until it finishes or control-C is pressed..
    try:
        valve1.set_flow(50, extend=1, retract=2)
        polling_wait(arduinos, duration=1.0)
        
        valve1.set_flow(-50, extend=1, retract=2)
        valve1.set_flow(50, extend=3, retract=4)
        polling_wait(arduinos, duration=1.0)

        valve1.set_flow(-50, extend=3, retract=4)
        polling_wait(arduinos, duration=1.0)

        valve1.set_flow(0, extend=1, retract=2)
        valve1.set_flow(0, extend=3, retract=4)
        polling_wait(arduinos, duration=1.0)
                
        valve2.set_flow(50, extend=1, retract=2)
        polling_wait(arduinos, duration=1.0)
        
        valve2.set_flow(-50, extend=1, retract=2)
        valve2.set_flow(50, extend=3, retract=4)
        polling_wait(arduinos, duration=1.0)

        valve2.set_flow(-50, extend=3, retract=4)
        polling_wait(arduinos, duration=1.0)

        valve2.set_flow(0, extend=1, retract=2)
        valve2.set_flow(0, extend=3, retract=4)        
        polling_wait(arduinos, duration=1.0)
            
    except KeyboardInterrupt:
        print("User interrupted operation.")

    # Shut down the connections.
    for arduino in arduinos:    
        arduino.close_serial_port()