#!/usr/bin/env python

"""\
test_client.py : sample code in Python to communicate with an Arduino running CNC_Shield_Server

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

"""

#================================================================
from __future__ import print_function
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 CncShieldClient(object):
    """Class to manage a connection to a CNC_Shield_Server running on a
    serial-connected Arduino. 

    :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] == 'txyz':
                self.arduino_time = int(elements[1])
                self.position = [int(s) for s in elements[2:]]

            elif elements[0] == 'awake':
                self.awake = True
                
            elif elements[0] == '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+'\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 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] absolute position (specified in microsteps) and wait until completion.

        :param position: a list or tuple with at least three elements
        """
        
        self._send_command( "goto %d %d %d" % tuple(position))
        self.target = position
        
        while self.position[0] != position[0] or self.position[1] != position[1] or self.position[2] != position[2]:
            self._wait_for_input()
            if self.verbose:
                print ("Position:", self.position)

        return

#================================================================

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

    # Initialize the command parser.
    parser = argparse.ArgumentParser( description = """Simple test client to send data to the CNC_Shield_Server 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( '--extra', action='store_true', help='Unused.')
    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()

    # 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 = CncShieldClient(**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()

        print("Beginning movement sequence.")
        client.motor_enable()
        client.move_to([1000, 2000, 3000])
        client.move_to([0, 0, 0])
        client.move_to([1000, 2000, 3000])
        client.move_to([0, 0, 0])
        client.move_to([3000, 2000, 1000])
        client.move_to([0, 0, 0])
        client.move_to([3000, 2000, 1000])
        client.move_to([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()