#!/usr/bin/env python3

"""\
suitcase_motion_demo.py : sample code in Python to communicate with an Arduino running StepperWinch

No copyright, 2021, Garth Zeglin.  This file is explicitly placed in the public domain.

"""

#================================================================
import argparse
import time

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

#================================================================
class StepperWinchClient(object):
    """Class to manage a connection to a serial-connected Arduino running the StepperWinch script.
    
    :param port: the name of the serial port device
    :param verbose: flag to increase console output
    :param debug: flag to print raw inputs on sconsole
    :param kwargs: collect any unused keyword arguments
    """
    
    def __init__(self, port=None, verbose=False, debug=False, **kwargs ):
        # initialize the client state
        self.arduino_time = 0
        self.position = [0, 0, 0]
        self.target   = [0, 0, 0]
        self.verbose = verbose
        self.debug   = debug
        self.awake   = False

        # open the serial port, which should also reset the Arduino
        self.port = serial.Serial( port, 115200, timeout=5 )
       
        if self.verbose:
            print("Opened serial port named", self.port.name)
            print("Sleeping briefly while Arduino boots...")

        # wait briefly for the Arduino to finish booting
        time.sleep(2)   # units are seconds

        # throw away any extraneous input
        self.port.flushInput()
        
        return
    
    def close(self):
        """Shut down the serial connection to the Arduino, after which this object may no longer be used."""
        self.port.close()
        self.port = None
        return
    
    def _wait_for_input(self):
        line = self.port.readline().rstrip()
        
        if line:
            elements = line.split()
            if self.debug:
                print("Received: '%s'" % line)

            if elements[0] == b'txyza':
                self.arduino_time = int(elements[1])
                self.position = [int(s) for s in elements[2:]]

            elif elements[0] == b'awake':
                self.awake = True
                
            elif elements[0] == b'dbg':
                print("Received debugging message:", line)
                
            else:
                print("Unknown status message: ", line)
            
        return

    def _send_command(self, string):
        if self.verbose:
            print("Sending: ", string)
        self.port.write(string.encode() + b'\n')
        return

    def motor_enable( self, value=True):
        """Issue a command to enable or disable the stepper motor drivers."""
        
        self._send_command( "enable 1" if value is True else "enable 0" )
        return

    def set_freq_damping(self, freq, damping):
        """Issue a command to set the second-order model gains."""
        self._send_command("g xyza %f %f" % (freq, damping))
        return
            
    def wait_for_wakeup(self):
        """Issue a status query and wait until an 'awake' status has been received."""
        while self.awake is False:
            self._send_command("ping")
            self._wait_for_input()

    def move_to(self, position):
        """Issue a command to move to a [x, y, z, a] absolute position (specified in microsteps) and wait until completion.

        :param position: a list or tuple with at least four elements
        """
        
        self._send_command("a xyza %d %d %d %d" % tuple(position))
        self.target = position

        # wait for all reported positions to be close to the request
        moving = True
        while moving:
            self._wait_for_input()
            if self.verbose:
                print ("Position:", self.position)
            moving = any([abs(pos - target) > 5 for pos, target in zip(self.position, self.target)])

        return

#================================================================
cmd_desc =  "Simple test client to send data to the StepperWinch firmware on an Arduino."

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

    # Initialize the command parser.
    parser = argparse.ArgumentParser(description = cmd_desc)
    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( '--extra', action='store_true', help='Unused.')
    parser.add_argument( '-p', '--port', default='/dev/cu.usbmodem14301',
                         help='Specify the name of the Arduino serial port device (default is %(default)s.)')

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

    # Convert the Namespace to a set of keyword arguments and initialize the
    # client object.  Note that it will ignore extraneous values, e.g., from
    # additional command argument inputs unneeded by this object.
    client = StepperWinchClient(**vars(args))

    # Begin the motion sequence.  This may be safely interrupted by the user pressing Control-C.
    try:
        print("Waiting for wakeup.")
        client.wait_for_wakeup()
        client.set_freq_damping(1.0, 0.5)
        
        while True:
            print("Beginning movement sequence.")
            client.motor_enable()
            client.move_to([100, 200, 300, 400])
            client.move_to([0, 0, 0, 0])
            client.move_to([4000, 1000, 2000, 3000])
            client.move_to([0, 0, 0, 0])
            client.move_to([400, 300, 200, 100])
            client.move_to([0, 0, 0, 0])
            client.move_to([3000, 4000, 1000, 2000])
            client.move_to([0, 0, 0, 0])

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

    # Issue a command to turn off the drivers, then shut down the connection.
    client.motor_enable(False)
    client.close()