#!/usr/bin/env python
"""\
animation_example.py : example command-line program to communicate with an Arduino over a serial port 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

################################################################
def update_animation_state(now):
    """Stub for a function called periodically to evaluate the sensor inputs and generate the next cycle of animation outputs.
    
    :param now: current wall clock time
    """
    
    pass

################################################################
def event_loop(arduinos):
    """Prototype for an event-processing loop which manages I/O on multiple serial ports.

    :param arduinos: list of Arduino protocol objects
    """

    polling_interval = 0.5
    polling_timer = polling_interval
    last_time = time.time()
    
    while True:
        # Check which Arduinos have data available, using select with a short timeout.
        ready = select.select(arduinos,[], [], max(polling_timer, 0.0))

        # Process messages on each Arduino port with available data. 
        for arduino in ready[0]: arduino.wait_for_message()
          
        # Check whether enough time has elapsed to run a polling cycle, and recalculate the next timeout.
        now = time.time()
        polling_timer -= (now - last_time) # subtract elapsed time
        last_time = now
        
        # When the polling interval has expired:
        if polling_timer <= 0:
            polling_timer += polling_interval

            # Apply any polled logic here; this will be called at a constant average rate.

            # Print a debugging message stream.
            for arduino in arduinos:
                print("arduino status: awake: %s, joints: %s" % (arduino.awake_time, arduino.joints))

            # Callback to update the animation based on any new data.
            update_animation_state(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.' )
    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()

    # Extract specific command line options into a keyword argument dictionary.
    kwargs = { 'port' : args.port, 'debug' : args.debug, 'verbose' : args.verbose }
    
    # Create a protocol object to manage the serial port.
    arduino = ValveControlProtocol(**kwargs)

    # Connect to the Arduino, which will also reset it.
    print("Connecting to Arduino.")
    arduino.open_serial_port()

    # issue a few test commands
    time.sleep(2)
    arduino.set_flow(50, extend=1, retract=2)
    time.sleep(1)
    arduino.set_flow(-50, extend=1, retract=2)
    arduino.set_flow(50, extend=3, retract=4)
    time.sleep(1)
    arduino.set_flow(-50, extend=3, retract=4)
    time.sleep(1)
    arduino.set_flow(0, extend=1, retract=2)
    arduino.set_flow(0, extend=3, retract=4)        
    
    # Now run the event loop until it exits or control-C is pressed.
    try:
        event_loop([arduino])
            
    except KeyboardInterrupt:
        print("User interrupted operation.")

    # Shut down the connection.
    arduino.close_serial_port()
        

#!/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()