# Copyright (c) 2016-2017 Anki, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

'''
Actions encapsulate specific high-level tasks that the Cozmo robot can perform.
They have a definite beginning and end.

These tasks include picking up an object, rotating in place, saying text, etc.

Actions are usually triggered by a call to a method on the
:class:`cozmo.robot.Robot` class such as :meth:`~cozmo.robot.Robot.turn_in_place`

The call will return an object that subclasses :class:`Action` that can be
used to cancel the action, or be observed to wait or be notified when the
action completes (or fails) by calling its
:meth:`~cozmo.event.Dispatcher.wait_for` or
:meth:`~cozmo.event.Dispatcher.add_event_handler` methods.


Warning:
    Unless you pass ``in_parallel=True`` when starting the action, no other
    action can be active at the same time.  Attempting to trigger a non-parallel
    action when another action is already in progress will result in a
    :class:`~cozmo.exceptions.RobotBusy` exception being raised.

    When using ``in_parallel=True`` you may see an action fail with the result
    :attr:`ActionResults.TRACKS_LOCKED` - this indicates that another in-progress
    action has already locked that movement track (e.g. two actions cannot
    move the head at the same time).
'''

# __all__ should order by constants, event classes, other classes, functions.
__all__ = ['ACTION_IDLE', 'ACTION_RUNNING', 'ACTION_SUCCEEDED',
           'ACTION_FAILED', 'ACTION_ABORTING',
           'EvtActionStarted', 'EvtActionCompleted', 'Action', 'ActionResults']


from collections import namedtuple
import sys

from . import logger

from . import event
from . import exceptions
from ._clad import _clad_to_engine_iface, _clad_to_engine_cozmo, _clad_to_game_cozmo


#: string: Action idle state
ACTION_IDLE = 'action_idle'

#: string: Action running state
ACTION_RUNNING = 'action_running'

#: string: Action succeeded state
ACTION_SUCCEEDED = 'action_succeeded'

#: string: Action failed state
ACTION_FAILED = 'action_failed'

#: string: Action failed state
ACTION_ABORTING = 'action_aborting'

_VALID_STATES = {ACTION_IDLE, ACTION_RUNNING, ACTION_SUCCEEDED, ACTION_FAILED, ACTION_ABORTING}


class _ActionResult(namedtuple('_ActionResult', 'name id')):
    # Tuple mapping between CLAD ActionResult name and ID
    # All instances will be members of ActionResults

    # Keep _ActionResult as lightweight as a normal namedtuple
    __slots__ = ()

    def __str__(self):
        return 'ActionResults.%s' % self.name


class ActionResults:
    """The possible result values for an Action.

    An Action's result is set when the action completes.
    """

    #: Action completed successfully.
    SUCCESS = _ActionResult("SUCCESS", _clad_to_game_cozmo.ActionResult.SUCCESS)

    #: Action is still running.
    RUNNING = _ActionResult("RUNNING", _clad_to_game_cozmo.ActionResult.RUNNING)

    #: Action was cancelled (e.g. via :meth:`~cozmo.robot.Robot.abort_all_actions` or
    #: :meth:`Action.abort`).
    CANCELLED = _ActionResult("CANCELLED", _clad_to_game_cozmo.ActionResult.CANCELLED)

    #: Action aborted itself (e.g. had invalid attributes, or a runtime failure).
    ABORT = _ActionResult("ABORT", _clad_to_game_cozmo.ActionResult.ABORT)

    #: Animation Action aborted itself (e.g. there was an error playing the animation).
    ANIM_ABORTED = _ActionResult("ANIM_ABORTED", _clad_to_game_cozmo.ActionResult.ANIM_ABORTED)

    #: There was an error related to vision markers.
    BAD_MARKER = _ActionResult("BAD_MARKER", _clad_to_game_cozmo.ActionResult.BAD_MARKER)

    #: There was a problem with the Object ID provided (e.g. there is no Object with that ID).
    BAD_OBJECT = _ActionResult("BAD_OBJECT", _clad_to_game_cozmo.ActionResult.BAD_OBJECT)

    #: There was a problem with the Pose provided.
    BAD_POSE = _ActionResult("BAD_POSE", _clad_to_game_cozmo.ActionResult.BAD_POSE)

    # (Undocumented) The SDK-provided tag was bad (shouldn't occur - would indicate a bug in the SDK)
    BAD_TAG = _ActionResult("BAD_TAG", _clad_to_game_cozmo.ActionResult.BAD_TAG)

    # (Undocumented) Shouldn't occur outside of factory
    FAILED_SETTING_CALIBRATION = _ActionResult("FAILED_SETTING_CALIBRATION",
                                               _clad_to_game_cozmo.ActionResult.FAILED_SETTING_CALIBRATION)

    #: There was an error following the planned path.
    FOLLOWING_PATH_BUT_NOT_TRAVERSING = _ActionResult("FOLLOWING_PATH_BUT_NOT_TRAVERSING",
                                                      _clad_to_game_cozmo.ActionResult.FOLLOWING_PATH_BUT_NOT_TRAVERSING)

    #: The action was interrupted by another Action or Behavior.
    INTERRUPTED = _ActionResult("INTERRUPTED", _clad_to_game_cozmo.ActionResult.INTERRUPTED)

    #: The Up Axis of a carried object doesn't match the desired placement pose.
    MISMATCHED_UP_AXIS = _ActionResult("MISMATCHED_UP_AXIS", _clad_to_game_cozmo.ActionResult.MISMATCHED_UP_AXIS)

    #: No valid Animation name was found.
    NO_ANIM_NAME = _ActionResult("NO_ANIM_NAME", _clad_to_game_cozmo.ActionResult.NO_ANIM_NAME)

    #: An invalid distance value was given.
    NO_DISTANCE_SET = _ActionResult("NO_DISTANCE_SET", _clad_to_game_cozmo.ActionResult.NO_DISTANCE_SET)

    #: There was a problem with the Face ID (e.g. Cozmo doesn't no where it is).
    NO_FACE = _ActionResult("NO_FACE", _clad_to_game_cozmo.ActionResult.NO_FACE)

    #: No goal pose was set.
    NO_GOAL_SET = _ActionResult("NO_GOAL_SET", _clad_to_game_cozmo.ActionResult.NO_GOAL_SET)

    #: No pre-action poses were found (e.g. could not get into position).
    NO_PREACTION_POSES = _ActionResult("NO_PREACTION_POSES",
                                       _clad_to_game_cozmo.ActionResult.NO_PREACTION_POSES)

    #: No object is being carried, but the action requires one.
    NOT_CARRYING_OBJECT_ABORT = _ActionResult("NOT_CARRYING_OBJECT_ABORT",
                                              _clad_to_game_cozmo.ActionResult.NOT_CARRYING_OBJECT_ABORT)

    #: Initial state of an Action to indicate it has not yet started.
    NOT_STARTED = _ActionResult("NOT_STARTED", _clad_to_game_cozmo.ActionResult.NOT_STARTED)

    #: No sub-action was provided.
    NULL_SUBACTION = _ActionResult("NULL_SUBACTION", _clad_to_game_cozmo.ActionResult.NULL_SUBACTION)

    #: Cozmo was unable to plan a path.
    PATH_PLANNING_FAILED_ABORT = _ActionResult("PATH_PLANNING_FAILED_ABORT",
                                               _clad_to_game_cozmo.ActionResult.PATH_PLANNING_FAILED_ABORT)

    #: The object that Cozmo is attempting to pickup is unexpectedly moving (e.g
    #: it is being moved by someone else).
    PICKUP_OBJECT_UNEXPECTEDLY_MOVING = _ActionResult("PICKUP_OBJECT_UNEXPECTEDLY_MOVING",
                                                      _clad_to_game_cozmo.ActionResult.PICKUP_OBJECT_UNEXPECTEDLY_MOVING)

    #: The object that Cozmo thought he was lifting didn't start moving, so he
    #: must have missed.
    PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING = _ActionResult("PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING",
                                                          _clad_to_game_cozmo.ActionResult.PICKUP_OBJECT_UNEXPECTEDLY_NOT_MOVING)

    # (Undocumented) Shouldn't occur in SDK usage
    SEND_MESSAGE_TO_ROBOT_FAILED = _ActionResult("SEND_MESSAGE_TO_ROBOT_FAILED",
                                                 _clad_to_game_cozmo.ActionResult.SEND_MESSAGE_TO_ROBOT_FAILED)

    #: Cozmo is unexpectedly still carrying an object.
    STILL_CARRYING_OBJECT = _ActionResult("STILL_CARRYING_OBJECT",
                                          _clad_to_game_cozmo.ActionResult.STILL_CARRYING_OBJECT)

    #: The Action timed out before completing correctly.
    TIMEOUT = _ActionResult("TIMEOUT", _clad_to_game_cozmo.ActionResult.TIMEOUT)

    #: One or more animation tracks (Head, Lift, Body, Face, Backpack Lights, Audio)
    #: are already being used by another Action.
    TRACKS_LOCKED = _ActionResult("TRACKS_LOCKED", _clad_to_game_cozmo.ActionResult.TRACKS_LOCKED)

    #: There was an internal error related to an unexpected type of dock action.
    UNEXPECTED_DOCK_ACTION = _ActionResult("UNEXPECTED_DOCK_ACTION",
                                           _clad_to_game_cozmo.ActionResult.UNEXPECTED_DOCK_ACTION)

    # (Undocumented) Shouldn't occur outside of factory.
    UNKNOWN_TOOL_CODE = _ActionResult("UNKNOWN_TOOL_CODE",
                                      _clad_to_game_cozmo.ActionResult.UNKNOWN_TOOL_CODE)

    # (Undocumented) There was a problem in the subclass's update.
    UPDATE_DERIVED_FAILED = _ActionResult("UPDATE_DERIVED_FAILED",
                                          _clad_to_game_cozmo.ActionResult.UPDATE_DERIVED_FAILED)

    #: Cozmo did not see the expected result (e.g. unable to see cubes in their
    #: expected position after a related action).
    VISUAL_OBSERVATION_FAILED = _ActionResult("VISUAL_OBSERVATION_FAILED",
                                              _clad_to_game_cozmo.ActionResult.VISUAL_OBSERVATION_FAILED)

    #: The Action failed, but may succeed if retried.
    RETRY = _ActionResult("RETRY", _clad_to_game_cozmo.ActionResult.RETRY)

    #: Failed to get into position.
    DID_NOT_REACH_PREACTION_POSE = _ActionResult("DID_NOT_REACH_PREACTION_POSE",
                                                 _clad_to_game_cozmo.ActionResult.DID_NOT_REACH_PREACTION_POSE)

    #: Failed to follow the planned path.
    FAILED_TRAVERSING_PATH = _ActionResult("FAILED_TRAVERSING_PATH",
                                           _clad_to_game_cozmo.ActionResult.FAILED_TRAVERSING_PATH)

    #: The previous attempt to pick and place an object failed.
    LAST_PICK_AND_PLACE_FAILED = _ActionResult("LAST_PICK_AND_PLACE_FAILED",
                                               _clad_to_game_cozmo.ActionResult.LAST_PICK_AND_PLACE_FAILED)

    #: The required motor isn't moving so the action cannot complete.
    MOTOR_STOPPED_MAKING_PROGRESS = _ActionResult("MOTOR_STOPPED_MAKING_PROGRESS",
                                                _clad_to_game_cozmo.ActionResult.MOTOR_STOPPED_MAKING_PROGRESS)

    #: Not carrying an object when it was expected, but may succeed if the action is retried.
    NOT_CARRYING_OBJECT_RETRY = _ActionResult("NOT_CARRYING_OBJECT_RETRY",
                                              _clad_to_game_cozmo.ActionResult.NOT_CARRYING_OBJECT_RETRY)

    #: Cozmo was unable to plan a path, but may succeed if the action is retried.
    PATH_PLANNING_FAILED_RETRY = _ActionResult("PATH_PLANNING_FAILED_RETRY",
                                               _clad_to_game_cozmo.ActionResult.PATH_PLANNING_FAILED_RETRY)

    #: There is no room to place the object at the desired destination.
    PLACEMENT_GOAL_NOT_FREE = _ActionResult("PLACEMENT_GOAL_NOT_FREE",
                                            _clad_to_game_cozmo.ActionResult.PLACEMENT_GOAL_NOT_FREE)

    #: A retriable action failed more times than the ``num_retries`` limit passed in.
    REACHED_MAX_NUM_RETRIES = _ActionResult("REACHED_MAX_NUM_RETRIES", _clad_to_game_cozmo.ActionResult.REACHED_MAX_NUM_RETRIES)

    #: Cozmo failed to drive off the charger.
    STILL_ON_CHARGER = _ActionResult("STILL_ON_CHARGER", _clad_to_game_cozmo.ActionResult.STILL_ON_CHARGER)

    #: Cozmo's pitch is at an unexpected angle for the Action.
    UNEXPECTED_PITCH_ANGLE = _ActionResult("UNEXPECTED_PITCH_ANGLE", _clad_to_game_cozmo.ActionResult.UNEXPECTED_PITCH_ANGLE)

    # Internal support for finding the above enum-esque members by ID
    _id_to_action_result = dict()

    @classmethod
    def find_by_id(cls, id):
        return cls._id_to_action_result.get(id)


# The table above is hard-coded so that it can generate the related API docs.
# To ensure that it stays in sync with the underlying CLAD code, and that the
# naming and IDs are correct, we validate the table here. Missing entries
# are automatically added, but a warning is output to identify that they should
# be added later for documentation purposes.
for (_name, _id) in _clad_to_game_cozmo.ActionResult.__dict__.items():
    if not _name.startswith('_') and (_name != 'Count') and _id >= 0:
        attr = getattr(ActionResults, _name, None)
        if attr is not None:
            if attr.id != _id:
                sys.exit(
                    'Error incorrect definition for id %s=%s, (should =%s) - line should read:\n'
                    '%s = _ActionResult("%s", _clad_to_game_cozmo.ActionResult.%s)'
                    % (_name, attr.id, _id, _name, _name, _name))
        else:
            logger.warning('Missing definition for id %s - to document it add:\n'
                           '%s = _ActionResult("%s", _clad_to_game_cozmo.ActionResult.%s)'
                           % (_id, _name, _name, _name))
            setattr(ActionResults, _name, _ActionResult(_name, _id))


# populate ActionResults _id_to_behavior_type mapping
for (_name, _ar) in ActionResults.__dict__.items():
    if isinstance(_ar, _ActionResult):
        ActionResults._id_to_action_result[_ar.id] = _ar


class EvtActionStarted(event.Event):
    '''Triggered when a robot starts an action.'''
    action = "The action that started"


class EvtActionCompleted(event.Event):
    '''Triggered when a robot action has completed or failed.'''
    action = "The action that completed"
    state = 'The state of the action; either cozmo.action.ACTION_SUCCEEDED or cozmo.action.ACTION_FAILED'
    failure_code = 'A failure code such as "cancelled"'
    failure_reason = 'A human-readable failure reason'


class Action(event.Dispatcher):
    """An action holds the state of an in-progress robot action
    """

    # Note: This is used for aborting the action, UNKNOWN will cancel any action.
    # Override this for any Actions that have a single, known, type used in Engine
    # TODO: abort by ID instead, and remove _action_type
    _action_type = _clad_to_engine_cozmo.RobotActionType.UNKNOWN

    # We allow sub-classes of Action to optionally disable logging messages
    # related to those actions being aborted - this is useful for actions
    # that are aborted frequently (by design) and would otherwise spam the log
    _enable_abort_logging = True

    def __init__(self, *, conn, robot, **kw):
        super().__init__(**kw)
        #: :class:`~cozmo.conn.CozmoConnection`: The connection on which the action was sent.
        self.conn = conn

        #: :class:`~cozmo.robot.Robot`: Th robot instance executing the action.
        self.robot = robot

        self._action_id = None
        self._state = ACTION_IDLE
        self._failure_code = None
        self._failure_reason = None
        self._result = None
        self._completed_event = None
        self._completed_event_pending = False

    def __repr__(self):
        extra = self._repr_values()
        if len(extra) > 0:
            extra = ' '+extra
        if self._state == ACTION_FAILED:
            extra += (" failure_reason='%s' failure_code=%s result=%s" %
                      (self._failure_reason, self._failure_code, self.result))
        return '<%s state=%s%s>' % (self.__class__.__name__, self.state, extra)

    def _repr_values(self):
        return ''

    def _encode(self):
        raise NotImplementedError()

    def _start(self):
        self._state = ACTION_RUNNING
        self.dispatch_event(EvtActionStarted, action=self)

    def _set_completed(self, msg):
        self._state = ACTION_SUCCEEDED
        self._completed_event_pending = False
        self._dispatch_completed_event(msg)

    def _dispatch_completed_event(self, msg):
        # Override to extra action-specific data from msg and generate
        # an action-specific completion event.  Do not call super if overriden.
        # Must generate a subclass of EvtActionCompleted.
        self._completed_event = EvtActionCompleted(action=self, state=self._state)
        self.dispatch_event(self._completed_event)

    def _set_failed(self, code, reason):
        self._state = ACTION_FAILED
        self._failure_code = code
        self._failure_reason = reason
        self._completed_event_pending = False
        self._completed_event = EvtActionCompleted(action=self, state=self._state,
                                              failure_code=code,
                                              failure_reason=reason)
        self.dispatch_event(self._completed_event)

    def _set_aborting(self):
        if not self.is_running:
            raise ValueError("Action isn't currently running")

        if self._enable_abort_logging:
            logger.info('Aborting action=%s', self)
        self._state = ACTION_ABORTING


    #### Properties ####

    @property
    def is_running(self):
        '''bool: True if the action is currently in progress.'''
        return self._state == ACTION_RUNNING

    @property
    def is_completed(self):
        '''bool: True if the action has completed (either succeeded or failed).'''
        return self._state in (ACTION_SUCCEEDED, ACTION_FAILED)

    @property
    def is_aborting(self):
        '''bool: True if the action is aborting (will soon be either succeeded or failed).'''
        return self._state == ACTION_ABORTING

    @property
    def has_succeeded(self):
        '''bool: True if the action has succeeded.'''
        return self._state == ACTION_SUCCEEDED

    @property
    def has_failed(self):
        '''bool: True if the action has failed.'''
        return self._state == ACTION_FAILED

    @property
    def failure_reason(self):
        '''tuple of (failure_code, failure_reason): Both values will be None if no failure has occurred.'''
        return (self._failure_code, self._failure_reason)

    @property
    def result(self):
        """An attribute of :class:`ActionResults`: The result of running the action."""
        return self._result

    @property
    def state(self):
        '''string: The current internal state of the action as a string.

        Will match one of the constants:
        :const:`ACTION_IDLE`
        :const:`ACTION_RUNNING`
        :const:`ACTION_SUCCEEDED`
        :const:`ACTION_FAILED`
        :const:`ACTION_ABORTING`
        '''
        return self._state


    #### Private Event Handlers ####

    def _recv_msg_robot_completed_action(self, evt, *, msg):
        result = msg.result
        types = _clad_to_game_cozmo.ActionResult

        self._result = ActionResults.find_by_id(result)
        if self._result is None:
            logger.error("ActionResults has no entry for result id %s", result)

        if result == types.SUCCESS:
            # dispatch to the specific type to extract result info
            self._set_completed(msg)

        elif result == types.RUNNING:
            # XXX what does one do with this? it seems to occur after a cancel request!
            logger.warning('Received "running" action notification for action=%s', self)
            self._set_failed('running', 'Action was still running')

        elif result == types.NOT_STARTED:
            # not sure we'll see this?
            self._set_failed('not_started', 'Action was not started')

        elif result == types.TIMEOUT:
            self._set_failed('timeout', 'Action timed out')

        elif result == types.TRACKS_LOCKED:
            self._set_failed('tracks_locked', 'Action failed due to tracks locked')

        elif result == types.BAD_TAG:
            # guessing this is bad
            self._set_failed('bad_tag', 'Action failed due to bad tag')
            logger.error("Received FAILURE_BAD_TAG for action %s", self)

        elif result == types.CANCELLED:
            self._set_failed('cancelled', 'Action was cancelled')

        elif result == types.INTERRUPTED:
            self._set_failed('interrupted', 'Action was interrupted')

        else:
            # All other results should fall under either the abort or retry
            # categories, determine the category by shifting the result
            result_category = result >> _clad_to_game_cozmo.ARCBitShift.NUM_BITS
            result_categories = _clad_to_game_cozmo.ActionResultCategory
            
            if result_category == result_categories.ABORT:
                self._set_failed('aborted', 'Action failed')
            elif result_category == result_categories.RETRY:
                self._set_failed('retry', 'Action failed but can be retried')
            else:
                # Shouldn't be able to get here
                self._set_failed('unknown', 'Action failed with unknown reason')
                logger.error('Received unknown action result status %s', msg)


    #### Public Event Handlers ####


    #### Commands ####

    def abort(self):
        '''Trigger the robot to abort the running action.

        Raises:
            ValueError if the action is not currently being executed.
        '''
        self.robot._action_dispatcher._abort_action(self)

    async def wait_for_completed(self, timeout=None):
        '''Waits for the action to complete.

        Args:
            timeout (int or None): Maximum time in seconds to wait for the event.
                Pass None to wait indefinitely.
        Returns:
            The :class:`EvtActionCompleted` event instance
        Raises:
            :class:`asyncio.TimeoutError`
        '''
        if self.is_completed:
            # Already complete
            return self._completed_event
        return await self.wait_for(EvtActionCompleted, timeout=timeout)

    def on_completed(self, handler):
        '''Triggers a handler when the action completes.

        Args:
            handler (callable): An event handler which accepts arguments
                suited to the :class:`EvtActionCompleted` event.
                See :meth:`cozmo.event.add_event_handler` for more information.
        '''
        return self.add_event_handler(EvtActionCompleted, handler)



class _ActionDispatcher(event.Dispatcher):
    _next_action_id = _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG

    def __init__(self, robot, **kw):
        super().__init__(**kw)
        self.robot = robot
        self._in_progress = {}
        self._aborting = {}

    def _get_next_action_id(self):
        # Post increment _current_action_id (and loop within the SDK_TAG range)
        next_action_id = self.__class__._next_action_id
        if self.__class__._next_action_id == _clad_to_game_cozmo.ActionConstants.LAST_SDK_TAG:
            self.__class__._next_action_id = _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG
        else:
            self.__class__._next_action_id += 1
        return next_action_id

    @property
    def aborting_actions(self):
        '''generator: yields each action that is currently aborting

        Returns:
            A generator yielding :class:`cozmo.action.Action` instances
        '''
        for _, action in self._aborting.items():
            yield action

    @property
    def has_in_progress_actions(self):
        '''bool: True if any SDK-triggered actions are still in progress.'''
        return len(self._in_progress) > 0

    @property
    def in_progress_actions(self):
        '''generator: yields each action that is currently in progress

        Returns:
            A generator yielding :class:`cozmo.action.Action` instances
        '''
        for _, action in self._in_progress.items():
            yield action

    async def wait_for_all_actions_completed(self):
        '''Waits until all actions are complete.

        In this case, all actions include not just in_progress actions but also
        include actions that we're aborting but haven't received a completed message
        for yet.
        '''
        while True:
            action = next(self.in_progress_actions, None)
            if action is None:
                action = next(self.aborting_actions, None)
            if action:
                await action.wait_for_completed()
            else:
                # all actions are now complete
                return

    def _send_single_action(self, action, in_parallel=False, num_retries=0):
        action_id = self._get_next_action_id()
        action.robot = self.robot
        action._action_id = action_id

        if self.has_in_progress_actions and not in_parallel:
            # Note - it doesn't matter if previous action was started as in_parallel,
            # starting any subsequent action with in_parallel==False will cancel
            # any previous actions, so we throw an exception here and require that
            # the client explicitly cancel or wait on earlier actions
            action = list(self._in_progress.values())[0]
            raise exceptions.RobotBusy('Robot is already performing %d action(s) %s' %
                                       (len(self._in_progress), action))

        if action.is_running:
            raise ValueError('Action is already running')

        if action.is_completed:
            raise ValueError('Action already ran')

        if in_parallel:
            position = _clad_to_game_cozmo.QueueActionPosition.IN_PARALLEL
        else:
            position = _clad_to_game_cozmo.QueueActionPosition.NOW

        qmsg = _clad_to_engine_iface.QueueSingleAction(
            robotID=self.robot.robot_id, idTag=action_id, numRetries=num_retries,
            position=position, action=_clad_to_engine_iface.RobotActionUnion())
        action_msg = action._encode()
        cls_name = action_msg.__class__.__name__
        # For some reason, the RobotActionUnion type uses properties with a lowercase
        # first character, instead of uppercase like all the other unions
        cls_name = cls_name[0].lower() + cls_name[1:]
        setattr(qmsg.action, cls_name, action_msg)
        self.robot.conn.send_msg(qmsg)
        self._in_progress[action_id] = action
        action._start()

    def _is_sdk_action_id(self, action_id):
        return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_SDK_TAG) 
                and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_SDK_TAG))

    def _is_engine_action_id(self, action_id):
        return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_ENGINE_TAG) 
                and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_ENGINE_TAG))

    def _is_game_action_id(self, action_id):
        return ((action_id >= _clad_to_game_cozmo.ActionConstants.FIRST_GAME_TAG) 
                and (action_id <= _clad_to_game_cozmo.ActionConstants.LAST_GAME_TAG))

    def _action_id_type(self, action_id):
        if self._is_sdk_action_id(action_id):
            return "sdk"
        elif self._is_engine_action_id(action_id):
            return "engine"
        elif self._is_game_action_id(action_id):
            return "game"
        else:
            return "unknown"

    def _recv_msg_robot_completed_action(self, evt, *, msg):
        action_id = msg.idTag
        is_sdk_action = self._is_sdk_action_id(action_id)
        action = self._in_progress.get(action_id)
        was_aborted = False
        if action is None:
            action = self._aborting.get(action_id)
            was_aborted = action is not None
        if action is None:
            if is_sdk_action:
                logger.error('Received completed action message for unknown SDK action_id=%s', action_id)
            return
        else:
            if not is_sdk_action:
                action_id_type = self._action_id_type(action_id)
                logger.error('Received completed action message for sdk-known %s action_id=%s (was_aborted=%s)',
                             action_id_type, action_id, was_aborted)

        action._completed_event_pending = True

        if was_aborted:
            if action._enable_abort_logging:
                logger.debug('Received completed action message for aborted action=%s', action)
            del self._aborting[action_id]
        else:
            logger.debug('Received completed action message for in-progress action=%s', action)
            del self._in_progress[action_id]
        # XXX This should generate a real event, not a msg
        # Should also dispatch to self so the parent can be notified.
        action.dispatch_event(evt)

    def _abort_action(self, action):
        # Mark this in-progress action as aborting - it should get a "Cancelled"
        # message back in the next engine tick, and can basically be considered
        # cancelled from now.
        action._set_aborting()

        if action._completed_event_pending:
            # The action was marked as still running but the ActionDispatcher
            # has already received a completion message (and removed it from
            # _in_progress) - the action is just waiting to receive a
            # robot_completed_action message that is still being dispatched
            # via asyncio.ensure_future
            logger.debug('Not sending abort for action=%s to engine as it just completed', action)
        else:
            # move from in-progress to aborting dicts
            self._aborting[action._action_id] = action
            del self._in_progress[action._action_id]

            msg = _clad_to_engine_iface.CancelActionByIdTag(idTag=action._action_id,
                                                            robotID=self.robot.robot_id)
            self.robot.conn.send_msg(msg)

    def _abort_all_actions(self):
        # Mark any in-progress actions as aborting - they should get a "Cancelled"
        # message back in the next engine tick, and can basically be considered
        # cancelled from now.
        actions_to_abort = self._in_progress
        self._in_progress = {}
        for action_id, action in actions_to_abort.items():
            action._set_aborting()
            self._aborting[action_id] = action

        logger.info('Sending abort request for all actions')
        # RobotActionType.UNKNOWN is a wildcard that matches all actions when cancelling.
        msg = _clad_to_engine_iface.CancelAction(robotID=self.robot.robot_id,
                                                 actionType=_clad_to_engine_cozmo.RobotActionType.UNKNOWN)
        self.robot.conn.send_msg(msg)

