Source code for

# -*- coding: utf-8 -*-

import serial
import logging
import operator
import itertools
import threading

from collections import namedtuple, OrderedDict
from contextlib import contextmanager

from ..conversion import (dxl_code_all, dxl_decode_all, decode_error,

logger = logging.getLogger(__name__)
# With this logger you should always provide as extra:
# - the port
# - the baudrate
# - the timeout

_DxlControl = namedtuple('_DxlControl', ('name',
                                         'address', 'length', 'nb_elem',
                                         'dxl_to_si', 'si_to_dxl',
                                         'getter_name', 'setter_name'))

class _DxlAccess(object):
    readonly, writeonly, readwrite = range(3)

class AbstractDxlIO(object):
    """ Low-level class to handle the serial communication with the robotis motors. """

    __used_ports = set()
    __controls = []
    _protocol = None

    def get_used_ports(cls):
        return list(cls.__used_ports)

    # MARK: - Open, Close and Flush the communication

    def __init__(self,
                 port, baudrate=1000000, timeout=0.05,
        """ At instanciation, it opens the serial port and sets the communication parameters.

            :param string port: the serial port to use (e.g. Unix (/dev/tty...), Windows (COM...)).
            :param int baudrate: default for new motors: 57600, for PyPot motors: 1000000
            :param float timeout: read timeout in seconds
            :param bool use_sync_read: whether or not to use the SYNC_READ instruction
            :param error_handler: set a handler that will receive the different errors
            :type error_handler: :py:class:`~pypot.dynamixel.error.DxlErrorHandler`
            :param bool convert: whether or not convert values to units expressed in the standard system

            :raises: :py:exc:`` if the port is already used.

        self._known_models = {}
        self._known_mode = {}

        self._sync_read = use_sync_read
        self._error_handler = error_handler_cls() if error_handler_cls else None
        self._convert = convert

        self._serial_lock = threading.Lock(), baudrate, timeout)

    def __enter__(self):
        return self

    def __del__(self):

    def __exit__(self, exc_type, exc_value, traceback):

    def __repr__(self):
        return ('<DXL IO: closed={self.closed}, '
                'port="{self.port}", '
                'baudrate={self.baudrate}, '

    def open(self, port, baudrate=1000000, timeout=0.05):
        """ Opens a new serial communication (closes the previous communication if needed).

            :raises: :py:exc:`` if the port is already used.

        self._open(port, baudrate, timeout)"Opening port '%s'", self.port,
                    extra={'port': port,
                           'baudrate': baudrate,
                           'timeout': timeout})

    def _open(self, port, baudrate, timeout, max_recursion=500):
        # Tries to connect to port until it succeeds to ping any motor on the bus.
        # This is  used to circumvent a bug with the driver for the USB2AX on Mac.
        # Warning: If no motor is connected on the bus, this will run forever!!!
        import platform
        # import time
        import pypot.utils.pypot_time as time

        for i in range(max_recursion):

            with self._serial_lock:

                if port in self.__used_ports:
                    raise DxlError('Another instance of pypot use the port {}. You should restart your Python kernel to pass through this issue.'.format(port))

                # Dirty walkaround to fix a strange bug.
                # Observed with the USB2AX on Linux with pyserial 2.7
                # We have to first open/close the port in order to make it work
                # at 1Mbauds
                if platform.system() == 'Linux' and self._sync_read:
                    self._serial = serial.Serial(port, 9600)

                self._serial = serial.Serial(port, baudrate, timeout=timeout)

            if (platform.system() == 'Darwin' and
           == 'v1' and self._sync_read):
                if not
            raise DxlError('could not connect to the port {}'.format(self.port))

    def close(self, _force_lock=False):
        """ Closes the serial communication if opened. """
        if not self.closed:
            with self.__force_lock(_force_lock) or self._serial_lock:

  "Closing port '%s'", self.port,
                        extra={'port': self.port,
                               'baudrate': self.baudrate,
                               'timeout': self.timeout})

    def flush(self, _force_lock=False):
        """ Flushes the serial communication (both input and output). """
        if self.closed:
            raise DxlError('attempt to flush a closed serial communication')

        with self.__force_lock(_force_lock) or self._serial_lock:

    def __force_lock(self, condition):
        return with_True() if condition else False

    # MARK: Properties of the serial communication

    def port(self):
        """ Port used by the :class:``. If set, will re-open a new connection. """
        return self._serial.port

    def port(self, value):, self.baudrate, self.timeout)

    def baudrate(self):
        """ Baudrate used by the :class:``. If set, will re-open a new connection. """
        return self._serial.baudrate

    def baudrate(self, value):, value, self.timeout)

    def timeout(self):
        """ Timeout used by the :class:``. If set, will re-open a new connection. """
        return self._serial.timeout

    def timeout(self, value):, self.baudrate, value)

    def closed(self):
        """ Checks if the connection is closed. """
        return not (hasattr(self, '_serial') and self._serial.isOpen())

    # MARK: - Motor discovery

    def ping(self, id):
        """ Pings the motor with the specified id.

            .. note:: The motor id should always be included in [0, 253]. 254 is used for broadcast.

        pp = self._protocol.DxlPingPacket(id)

            self._send_packet(pp, error_handler=None)
            return True
        except DxlTimeoutError:
            return False

    def scan(self, ids=range(254)):
        """ Pings all ids within the specified list, by default it finds all the motors connected to the bus. """
        return [id for id in ids if]

    # MARK: - Specific Getter / Setter

    def get_model(self, ids):
        """ Gets the model for the specified motors. """
        to_get_ids = [i for i in ids if i not in self._known_models]
        models = [dxl_to_model(m) for m in self._get_model(to_get_ids, convert=False)]
        self._known_models.update(zip(to_get_ids, models))

        return tuple(self._known_models[id] for id in ids)

    def change_id(self, new_id_for_id):
        """ Changes the id of the specified motors (each id must be unique on the bus). """
        if len(set(new_id_for_id.values())) < len(new_id_for_id):
            raise ValueError('each id must be unique.')

        for new_id in new_id_for_id.itervalues():
                raise ValueError('id {} is already used.'.format(new_id))


        for motor_id, new_id in new_id_for_id.iteritems():
            if motor_id in self._known_models:
                self._known_models[new_id] = self._known_models[motor_id]
                del self._known_models[motor_id]
            if motor_id in self._known_mode:
                self._known_mode[new_id] = self._known_mode[motor_id]
                del self._known_mode[motor_id]

    def change_baudrate(self, baudrate_for_ids):
        """ Changes the baudrate of the specified motors. """

        for motor_id in baudrate_for_ids:
            if motor_id in self._known_models:
                del self._known_models[motor_id]
            if motor_id in self._known_mode:
                del self._known_mode[motor_id]

    def get_status_return_level(self, ids, **kwargs):
        """ Gets the status level for the specified motors. """
        convert = kwargs['convert'] if 'convert' in kwargs else self._convert
        srl = []
        for id in ids:
                srl.extend(self._get_status_return_level((id, ),
                                                         error_handler=None, convert=convert))
            except DxlTimeoutError as e:
                    srl.append('never' if convert else 0)
                    if self._error_handler:
                        return ()
                        raise e

        return tuple(srl)

    def set_status_return_level(self, srl_for_id, **kwargs):
        """ Sets status return level to the specified motors. """
        convert = kwargs['convert'] if 'convert' in kwargs else self._convert
        if convert:
            srl_for_id = dict(zip(srl_for_id.keys(),
                                  [('never', 'read', 'always').index(s) for s in srl_for_id.values()]))
        self._set_status_return_level(srl_for_id, convert=False)

    def switch_led_on(self, ids):
        """ Switches on the LED of the motors with the specified ids. """
        self._set_LED(dict(zip(ids, itertools.repeat(True))))

    def switch_led_off(self, ids):
        """ Switches off the LED of the motors with the specified ids. """
        self._set_LED(dict(zip(ids, itertools.repeat(False))))

    def enable_torque(self, ids):
        """ Enables torque of the motors with the specified ids. """
        self._set_torque_enable(dict(zip(ids, itertools.repeat(True))))

    def disable_torque(self, ids):
        """ Disables torque of the motors with the specified ids. """
        self._set_torque_enable(dict(zip(ids, itertools.repeat(False))))

    def get_pid_gain(self, ids, **kwargs):
        """ Gets the pid gain for the specified motors. """
        return tuple([tuple(reversed(t)) for t in self._get_pid_gain(ids, **kwargs)])

    def set_pid_gain(self, pid_for_id, **kwargs):
        """ Sets the pid gain to the specified motors. """
        pid_for_id = dict(itertools.izip(pid_for_id.iterkeys(),
                                         [tuple(reversed(t)) for t in pid_for_id.values()]))
        self._set_pid_gain(pid_for_id, **kwargs)

    # MARK: - Generic Getter / Setter

    def get_control_table(self, ids, **kwargs):
        """ Gets the full control table for the specified motors.

            ..note:: This function requires the model for each motor to be known. Querring this additional information might add some extra delay.

        error_handler = kwargs['error_handler'] if ('error_handler' in kwargs) else self._error_handler
        convert = kwargs['convert'] if ('convert' in kwargs) else self._convert

        bl = ('goal position speed load', 'present position speed load')
        controls = [c for c in self._AbstractDxlIO__controls if not in bl]

        res = []

        for id, model in zip(ids, self.get_model(ids)):
            controls = [c for c in controls if model in c.models]

            controls = sorted(controls, key=lambda c: c.address)

            address = controls[0].address
            length = controls[-1].address + controls[-1].nb_elem * controls[-1].length

            rp = self._protocol.DxlReadDataPacket(id, address, length)
            sp = self._send_packet(rp, error_handler=error_handler)

            d = OrderedDict()
            for c in controls:
                v = dxl_decode_all(sp.parameters[c.address:c.address + c.nb_elem * c.length], c.nb_elem)
                d[] = c.dxl_to_si(v, model) if convert else v


        return tuple(res)

    def _generate_accessors(cls, control):

        if control.access in (_DxlAccess.readonly, _DxlAccess.readwrite):
            def my_getter(self, ids, **kwargs):
                return self._get_control_value(control, ids, **kwargs)

            func_name = control.getter_name if control.getter_name else 'get_{}'.format(' ', '_'))
            func_name = '_{}'.format(func_name) if hasattr(cls, func_name) else func_name
            my_getter.__doc__ = 'Gets {} from the specified motors.'.format(
            my_getter.__name__ = func_name
            setattr(cls, func_name, my_getter)

        if control.access in (_DxlAccess.writeonly, _DxlAccess.readwrite):
            def my_setter(self, value_for_id, **kwargs):
                self._set_control_value(control, value_for_id, **kwargs)

            func_name = control.setter_name if control.setter_name else 'set_{}'.format(' ', '_'))
            func_name = '_{}'.format(func_name) if hasattr(cls, func_name) else func_name
            my_setter.__doc__ = 'Sets {} to the specified motors.'.format(
            my_setter.__name__ = func_name
            setattr(cls, func_name, my_setter)

    def _get_control_value(self, control, ids, **kwargs):
        if not ids:
            return ()

        error_handler = kwargs['error_handler'] if ('error_handler' in kwargs) else self._error_handler
        convert = kwargs['convert'] if ('convert' in kwargs) else self._convert

        if self._sync_read and len(ids) > 1:
            rp = self._protocol.DxlSyncReadPacket(ids, control.address,
                                                  control.length * control.nb_elem)

            with self._serial_lock:
                sp = self._send_packet(rp,
                if not sp:
                    return ()

                if == 'v1':
                    values = sp.parameters

                elif == 'v2':
                    values = list(sp.parameters)
                    for i in range(len(ids) - 1):
                            sp = self.__real_read(rp, _force_lock=True)
                        except (DxlTimeoutError, DxlCommunicationError):
                            return ()

                    if len(values) < len(ids):
                        return ()

            values = []
            for motor_id in ids:
                rp = self._protocol.DxlReadDataPacket(motor_id, control.address, control.length * control.nb_elem)
                sp = self._send_packet(rp, error_handler=error_handler)

                if not sp:
                    return ()


        values = list(itertools.izip(*([iter(values)] * control.length * control.nb_elem)))
        values = [dxl_decode_all(value, control.nb_elem) for value in values]

        if not values:
            return ()

        # when using SYNC_READ instead of getting a timeout
        # a non existing motor will "return" the maximum value
        if self._sync_read and == 'v1':
            max_val = 2 ** (8 * control.length) - 1
            if max_val in (itertools.chain(*values) if control.nb_elem > 1 else values):
                lost_ids = []
                for i, v in enumerate(itertools.chain(*values) if control.nb_elem > 1 else values):
                    if v == max_val:
                        lost_ids.append(ids[i // control.nb_elem])
                e = DxlTimeoutError(self, rp, list(set(lost_ids)))
                if self._error_handler:
                    return ()
                    raise e

        if convert:
            models = self.get_model(ids)
            if not models:
                return ()
            values = [control.dxl_to_si(v, m) for v, m in zip(values, models)]

        return tuple(values)

    def _set_control_value(self, control, value_for_id, **kwargs):
        if not value_for_id:

        convert = kwargs['convert'] if ('convert' in kwargs) else self._convert

        if convert:
            models = self.get_model(value_for_id.keys())
            if not models:

            value_for_id = dict(zip(value_for_id.keys(),
                                    map(control.si_to_dxl, value_for_id.values(), models)))

        data = []
        for motor_id, value in value_for_id.iteritems():
            data.extend(itertools.chain((motor_id, ),
                                        dxl_code_all(value, control.length, control.nb_elem)))

        wp = self._protocol.DxlSyncWritePacket(control.address, control.length * control.nb_elem, data)
        self._send_packet(wp, wait_for_status_packet=False)

    # MARK: - Send/Receive packet
    def __real_send(self, instruction_packet, wait_for_status_packet, _force_lock):
        if self.closed:
            raise DxlError('try to send a packet on a closed serial communication')

        logger.debug('Sending %s', instruction_packet,
                     extra={'port': self.port,
                            'baudrate': self.baudrate,
                            'timeout': self.timeout})

        with self.__force_lock(_force_lock) or self._serial_lock:

            data = instruction_packet.to_string()
            nbytes = self._serial.write(data)
            if len(data) != nbytes:
                raise DxlCommunicationError(self,
                                            'instruction packet not entirely sent',

            if not wait_for_status_packet:

            status_packet = self.__real_read(instruction_packet, _force_lock=True)

            logger.debug('Receiving %s', status_packet,
                         extra={'port': self.port,
                                'baudrate': self.baudrate,
                                'timeout': self.timeout})

            return status_packet

    def __real_read(self, instruction_packet, _force_lock):
        with self.__force_lock(_force_lock) or self._serial_lock:
            data =
            if not data:
                raise DxlTimeoutError(self, instruction_packet,

                header = self._protocol.DxlPacketHeader.from_string(data)
                data +=
                status_packet = self._protocol.DxlStatusPacket.from_string(data)

            except ValueError:
                msg = 'could not parse received data {}'.format(bytearray(data))
                raise DxlCommunicationError(self, msg, instruction_packet)

            return status_packet

    def _send_packet(self,
                     instruction_packet, wait_for_status_packet=True,

        if not error_handler:
            return self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)

            sp = self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)

            if sp and sp.error:
                errors = decode_error(sp.error)
                for e in errors:
                    handler_name = 'handle_{}'.format(e.lower().replace(' ', '_'))
                    f = operator.methodcaller(handler_name, instruction_packet)

            return sp

        except DxlTimeoutError as e:

        except DxlCommunicationError as e:

# MARK: - Dxl Errors
class DxlError(Exception):
    """ Base class for all errors encountered using :class:``. """

class DxlCommunicationError(DxlError):
    """ Base error for communication error encountered when using :class:``. """
    def __init__(self, dxl_io, message, instruction_packet):
        self.dxl_io = dxl_io
        self.message = message
        self.instruction_packet = instruction_packet

    def __str__(self):
        return '{self.message} after sending {self.instruction_packet}'.format(self=self)

class DxlTimeoutError(DxlCommunicationError):
    """ Timeout error encountered when using :class:``. """
    def __init__(self, dxl_io, instruction_packet, ids):
        DxlCommunicationError.__init__(self, dxl_io, 'timeout occured', instruction_packet)
        self.ids = ids

    def __str__(self):
        return 'motors {} did not respond after sending {}'.format(self.ids, self.instruction_packet)

def with_True():
    yield True