"""
LinuxCNC User Interface helper functions
"""

import linuxcnc

import sys
import time
import math


class LinuxCNC_Exception(Exception):
    pass


class LinuxCNC:
    """
    This class implements the main interface to LinuxCNC.
    """

    def __init__(self, command=None, status=None, error=None):
        """init docs"""
        self.command = command
        self.status = status
        self.error = error

        if not self.command:
            self.command = linuxcnc.command()

        if not self.status:
            self.status = linuxcnc.stat()

        if not self.error:
            self.error = linuxcnc.error_channel()


    def wait_for_linuxcnc_startup(self, timeout=10.0):

        """Poll the Status buffer waiting for it to look initialized,
        rather than just allocated (all-zero).  Returns on success, throws
        RuntimeError on failure."""

        start_time = time.time()
        while time.time() - start_time < timeout:
            self.status.poll()
            if (self.status.angular_units == 0.0) \
                or (self.status.axis_mask == 0) \
                or (self.status.cycle_time == 0.0) \
                or (self.status.exec_state != linuxcnc.EXEC_DONE) \
                or (self.status.interp_state != linuxcnc.INTERP_IDLE) \
                or (self.status.inpos == False) \
                or (self.status.linear_units == 0.0) \
                or (self.status.max_acceleration == 0.0) \
                or (self.status.max_velocity == 0.0) \
                or (self.status.program_units == 0.0) \
                or (self.status.rapidrate == 0.0) \
                or (self.status.state != linuxcnc.RCS_DONE) \
                or (self.status.task_state != linuxcnc.STATE_ESTOP):
                time.sleep(0.1)
            else:
                # looks good
                return

        # timeout, throw an exception
        raise RuntimeError


    def all_joints_homed(self, joints):
        """Arguments:

            'joints' is an array of booleans, indicating the joints to
            check for homed-ness.

        Returns True if all the specified joints are homed, False if
        any are unhomed."""

        self.status.poll()
        for i in range(0,9):
            if joints[i] and not self.status.homed[i]:
                return False
        return True


    def wait_for_home(self, joints, timeout=10.0):
        """Arguments:

            'joints' is an array of booleans, indicating the joints that
            are homing.

            'timeout' is a float, the number of seconds to wait for
            homing before giving up.

        Returns if all the specified joints homed before the timeout,
        raises LinuxCNC_Exception if the timeout expired first."""

        start_time = time.time()
        while (time.time() - start_time) < timeout:
            if self.all_joints_homed(joints):
                return
            time.sleep(0.1)

        raise LinuxCNC_Exception("timeout waiting for homing to complete:\nstatus.homed:" + str(self.status.homed) + "\nstatus.position:" + str(self.status.position))


    def wait_for_axis_to_stop(self, axis_letter, timeout=10.0):
        """Arguments:
            'axis_letter' is a single character from the set [xyzabcuvw]
            indicating the axis to wait for stillness on.

            'timeout' is a float, the number of seconds to wait for the
            axis to stop before giving up.

        Returns if the specified axis stops moving before the timeout
        expires.  Raises LinuxCNC_Exception if the timeout expires before
        the axis stops.
        """

        axis_letter = axis_letter.lower()
        axis_index = 'xyzabcuvw'.index(axis_letter)
        print("waiting for axis", axis_letter, "to stop")
        self.status.poll()
        start_time = time.time()
        prev_pos = self.status.position[axis_index]
        while (time.time() - start_time) < timeout:
            time.sleep(0.1)
            self.status.poll()
            new_pos = self.status.position[axis_index]
            if new_pos == prev_pos:
                return
            prev_pos = new_pos
        raise LinuxCNC_Exception("axis %s didn't stop jogging!\n" % axis_letter + "axis %s is at %.3f %.3f seconds after reaching target (prev_pos=%.3f)\n" % (axis_letter, self.status.position[axis_index], timeout, prev_pos))


    def jog_axis(self, axis_letter, target, vel=5.0, timeout=10.0):
        """Arguments:

            'axis_letter' is a single character from the set [xyzabcuvw]
            indicating an axis.

            'target' is a float indicating the location to jog to.

            'vel' is a float indicating the jog speed.

            'timeout' is a float, the number of seconds to wait for the
            jog to reach the target before giving up.

        This function uses the linuxcnc.command.jog() function to do
        a continuous jog of the specified axis towards the target, at
        the specified speed.  Stops the jog when the target is reached.
        Will overshoot some.  The function returns after the axis has
        stopped moving.

        If the axis does not reach the target before the timeout, or if
        any other axis moved, raises LinuxCNC_Exeption.
        """

        self.status.poll()
        axis_letter = axis_letter.lower()
        axis_index = 'xyzabcuvw'.index(axis_letter)
        start_pos = self.status.position

        print("jogging axis %s from %.3f to %.3f" % (axis_letter, start_pos[axis_index], target))

        if self.status.position[axis_index] < target:
            vel = abs(vel)
            done = lambda pos: pos > target
        else:
            vel = -1.0 * abs(vel)
            done = lambda pos: pos < target

        # the 0 here means "jog axis, not joint"
        self.command.jog(linuxcnc.JOG_CONTINUOUS, 0, axis_index, vel)

        start = time.time()
        while not done(self.status.position[axis_index]) and ((time.time() - start) < timeout):
            time.sleep(0.1)
            self.status.poll()

        # the 0 here means "jog axis, not joint"
        self.command.jog(linuxcnc.JOG_STOP, 0, axis_index)

        if not done(self.status.position[axis_index]):
            raise LinuxCNC_Exception("failed to jog axis %s to %.3f\n" % (axis_letter, target) + "timed out at %.3f after %.3f seconds" % (self.status.position[axis_index], timeout))

        print("    jogged axis %d past target %.3f" % (axis_index, target))

        self.wait_for_axis_to_stop(axis_letter)

        success = True
        for i in range(0, 9):
            if i == axis_index:
                continue;
            if start_pos[i] != self.status.position[i]:
                raise LinuxCNC_Exception("axis %s moved from %.3f to %.3f but should not have!" % ('xyzabcuvw'[i], start_pos[i], self.status.position[i]))


    def wait_for_axis_to_stop_at(self, axis_letter, target, timeout=10.0, tolerance = 0.0001):
        """
        Arguments:
            'axis_letter' is a single character from the set [xyzabcuvw]
            indicating the axis to wait for stillness on.

            'target' is a float, where the specified axis is trying to
            move to.

            'timeout' is a float, the number of seconds to wait for the
            axis to stop before giving up.

            'tolerance' is a float, how close to the target the axis
            has to be before we consider it there.

        This function polls the LinuxCNC Status buffer and monitors the
        position field, waiting for the specified axis to come to rest
        at the specified target.  If this does not happen before the
        timeout, the function raises LinuxCNC_Exception.
        """

        axis_letter = axis_letter.lower()
        axis_index = 'xyzabcuvw'.index(axis_letter)

        self.status.poll()
        start = time.time()

        while ((time.time() - start) < timeout):
            prev_pos = self.status.position[axis_index]
            self.status.poll()
            vel = self.status.position[axis_index] - prev_pos
            error = math.fabs(self.status.position[axis_index] - target)
            if (error < tolerance) and (vel == 0):
                print("axis %s stopped at %.3f" % (axis_letter, target))
                return
            time.sleep(0.1)
        raise LinuxCNC_Exception("timeout waiting for axis %s to stop at %.3f (pos=%.3f, vel=%.3f)" % (axis_letter, target, self.status.position[axis_index], vel))


    def wait_for_interp_state(self, target_state, timeout=10.0):
        """
        Arguments:
            'target_state' is the Interpreter state to wait for,
            one of linuxcnc.INTERP_IDLE, linuxcnc.INTERP_PAUSED,
            linuxcnc.INTERP_READING, or linuxcnc.INTERP_WAITING.

            'timeout' is a float, the number of seconds to wait for the
            Interpreter to reach the target state.

        This function polls the LinuxCNC Status buffer waiting for the
        specified Interpreter state.  If the timeout expires before
        the Interpreter reaches the specified status, it raises
        LinuxCNC_Exception.
        """

        start_time = time.time()
        while (time.time() - start_time) < timeout:
            self.status.poll()
            if self.status.interp_state == target_state:
                return
            time.sleep(0.001)

        if self.status.interp_state != target_state:
            raise LinuxCNC_Exception("interpreter state %d did not reach target state %d" % (self.state.interp_state, target_state))


    def wait_for_tool_in_spindle(self, expected_tool, timeout=10.0):
        """
        Arguments:
            'expected_tool', integer, the tool we're waiting to find in
            the spindle.

            'timeout', float, the number of seconds to wait for the
            specified tool to show up in the spindle.

        This function polls the LinuxCNC Status buffer, waiting for the
        specified tool to appear in the spindle.  If the timeout expires
        before the tool appears, it raises LinuxCNC_Exception.
        """

        start_time = time.time()
        while (time.time() - start_time) < timeout:
            time.sleep(0.1)
            self.status.poll()
            if self.status.tool_in_spindle == expected_tool:
                print("the Stat buffer's toolInSpindle reached the value of %d after %f seconds" % (expected_tool, time.time() - start_time))
                return
        raise LinuxCNC_Exception("the Stat buffer's toolInSpindle value is %d, expected %d" % (self.status.tool_in_spindle, expected_tool))