#!/usr/bin/env python
"""position_regulation.py

Apply proportional control using sensor feedback and 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 issue_command(port, flow, extend=1, retract=2, verbose=False):
    """Issue an open-loop set of flow commands for a valve pair attached to an
    actuator.  Returns immediately.  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 verbose: flag to enable printed debugging output
    :return: None
    """

    # Make sure the flow value is integral and bounded.
    flow = int(max(min(flow, 100),-100))

    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))

    return

################################################################
def feedback_cycle(port, target, gain=0.5, **kwargs):
    """Run one cycle of proportional feedback control for a sensor and valve pair
    attached to an actuator.  Returns immediately or after an optional delay.
    No return value.  Extra keyword arguments are passed along to issue_command().

    :param port: serial port stream handle
    :param target: goal position, specified as an integer in arbitrary sensor units
    :param gain: proportional gain scaling sensor units to actuator units
    :param verbose: flag to enable printed debugging output
    :return: None

    """

    # Wait for a line of sensor data to be received from the Arduino.
    raw_sensor_data = port.readline()

    # The data format is specified in ValveControl, and may need to be adjusted if the sketch changes.
    # This assumes it is a space-delimited set of six numbers:
    #   <raw-position-0> <calibrated-position-0> <pwm-output-0> <raw-position-1> <calibrated-position-1> <pwm-output-1>

    # Divide the line into a list of strings.
    value_strings = raw_sensor_data.split()

    # Parse just the first raw position as an integer.  This is simply the
    # unfiltered 10-bit value of analogRead() from the potentiometer.
    position = int(value_strings[0])

    # For simplicity, the target is assumed to be specified in the same units.
    # In practice, this value should be calibrated to reflect a real-world
    # measurement so that gains can be chosen independent of the sensor scale.
    position_error = target - position

    # A positive error is assumed to map to a positive flow, e.g., the target is more extended than the current position.
    # The scaling factor maps the arbitrary sensor unit to flow percentage unit.
    flow_command = int(gain*position_error)

    # Optionally print a report.
    if kwargs.get('verbose', False):
        print("position: %d, target: %d, error: %d,   " % (position, target, position_error), end='')

    # Send the computed command back to the valve controller.
    issue_command(port, flow_command, **kwargs)

    return

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

    # Initialize the command parser.
    parser = argparse.ArgumentParser(description = """Position regulation example 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('-p1', default=500, type=int, help='First position target.')
    parser.add_argument('-p2', default=600, type=int, help='Second position target.')
    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.
    kwargs = { 'verbose': args.verbose }
    try:
        print("Beginning movement sequence.")
        for i in range(100):
              feedback_cycle(port, target = args.p1, **kwargs)

        for i in range(100):
              feedback_cycle(port, target = args.p2, **kwargs)

        for i in range(100):
              feedback_cycle(port, target = args.p1, **kwargs)

        for i in range(100):
              feedback_cycle(port, target = args.p2, **kwargs)

        print("Movement complete.")

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

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