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

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

#================================================================
import logging
import os.path
import time

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

# initialize logging for this module
log = logging.getLogger(__name__)

#================================================================
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
        if os.path.exists(port):
            self.port = serial.Serial(port, 115200, timeout=5)
            if self.verbose:
                log.info("Opened serial port named %s", self.port.name)
                log.info("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()
        else:
            log.warning("Serial port %s not available, running offline.", port)
            self.port = None
        return

    def close(self):
        """Shut down the serial connection to the Arduino, after which this object may no longer be used."""
        if self.port is not None:
            self.port.close()
            self.port = None
        return

    def _wait_for_input(self):
        if self.port is None:
            return

        line = self.port.readline().rstrip()

        if line:
            elements = line.split()
            if self.debug:
                log.debug("Received: '%s'", line)

            if elements[0] == b'txyza':
                self.arduino_time = int(elements[1])
                self.position = [int(s) for s in elements[2:]]
                if self.verbose:
                    log.debug("Steppers at %s", self.position)

            elif elements[0] == b'awake':
                self.awake = True

            elif elements[0] == b'dbg':
                log.info("Received debugging message: %s", line)

            else:
                log.warning("Unknown status message: %s", line)

        return

    def poll_status(self):
        if self.port is not None:
            if self.port.in_waiting > 0:
                self._wait_for_input()

    def _send_command(self, string):
        if self.verbose:
            log.debug("Sending: %s", string)
        if self.port is not None:
            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")
            if self.port is not None:
                self._wait_for_input()
            else:
                self.awake = True # simulate

    def send_move(self, position):
        """Issue a command to move to a [x, y, z, a] absolute position (specified in microsteps) and return immediately.

        :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
        if self.verbose:
            log.debug("Steppers sent to %s", position)
        return

    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:
            if self.port is None:
                moving = False
            else:
                self._wait_for_input()
                if self.verbose:
                    log.debug("Position: %s", self.position)
                moving = any([abs(pos - target) > 5 for pos, target in zip(self.position, self.target)])

        return

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