from ingenialink.servo import Servo, SERVO_MODE, SERVO_STATE, SERVO_UNITS_ACC, \
    SERVO_UNITS_TORQUE, SERVO_UNITS_POS, SERVO_UNITS_VEL
from ingenialink.ipb.register import *
from ingenialink.constants import *
from ingenialink.exceptions import *
from ingenialink.ipb.dictionary import IPBDictionary
from .network import IPBNetwork
from ingenialink.utils._utils import *
from .._ingenialink import lib, ffi
from ingenialink.register import dtype_size

import xml.etree.ElementTree as ET
from xml.dom import minidom
import numpy as np
import io

import ingenialogger
logger = ingenialogger.get_logger(__name__)

DIST_NUMBER_SAMPLES = IPBRegister(
    identifier='', units='', subnode=0, address=0x00C4, cyclic='CONFIG',
    dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
)
DIST_DATA = IPBRegister(
    identifier='', units='', subnode=0, address=0x00B4, cyclic='CONFIG',
    dtype=REG_DTYPE.U16, access=REG_ACCESS.WO, reg_range=None
)

STORE_COCO_ALL = IPBRegister(
    identifier='', units='', subnode=0, address=0x06DB, cyclic='CONFIG',
    dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
)

RESTORE_COCO_ALL = IPBRegister(
    identifier='', units='', subnode=0, address=0x06DC, cyclic='CONFIG',
    dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
)

STATUS_WORD = IPBRegister(
    identifier='', units='', subnode=1, address=0x0011, cyclic='CONFIG',
    dtype=REG_DTYPE.U16, access=REG_ACCESS.RW, reg_range=None
)

STORE_MOCO_ALL_REGISTERS = {
    1: IPBRegister(
        identifier='', units='', subnode=1, address=0x06DB, cyclic='CONFIG',
        dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
    ),
    2: IPBRegister(
        identifier='', units='', subnode=2, address=0x06DB, cyclic='CONFIG',
        dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
    ),
    3: IPBRegister(
        identifier='', units='', subnode=3, address=0x06DB, cyclic='CONFIG',
        dtype=REG_DTYPE.U32, access=REG_ACCESS.RW, reg_range=None
    )
}


class IPBServo(Servo):
    """IPB Servo defines a general class for all IPB based slaves.

    Args:
        cffi_servo (CData): CData instance of the servo.
        cffi_net (CData): CData instance of the network.
        target (int, str): Target ID for the slave.
        dictionary_path (str): Path to the dictionary.

    """
    def __init__(self, cffi_servo, cffi_net, target, dictionary_path=None):
        self._cffi_servo = cffi_servo
        """CFFI instance of the servo."""
        self._cffi_network = cffi_net
        """CFFI instance of the network."""

        super(IPBServo, self).__init__(target)
        _dictionary_path = cstr(dictionary_path) if dictionary_path else ffi.NULL

        self._state_cb = {}
        self._emcy_cb = {}

        if not hasattr(self, '_errors') or not self._errors:
            self._errors = self._get_all_errors(_dictionary_path)

    @staticmethod
    def _get_all_errors(dictionary):
        """Obtain all errors defined in the dictionary.

        Args:
            dictionary: Path to the dictionary file.

        Returns:
            dict: Current errors definde in the dictionary.

        """
        errors = dict()
        if str(dictionary) != "<cdata 'void *' NULL>":
            tree = ET.parse(dictionary)
            for error in tree.iter("Error"):
                label = error.find(".//Label")
                id = int(error.attrib['id'], 0)
                errors[id] = [
                    error.attrib['id'],
                    error.attrib['affected_module'],
                    error.attrib['error_type'].capitalize(),
                    label.text
                ]
        return errors

    def get_reg(self, reg, subnode):
        """Obtain Register object and its identifier.

        Args:
            reg (IPBRegister, str): Register.
            subnode (int): Subnode.

        Returns:
            tuple (Register, string): Actual Register instance and its
                                        identifier.

        """
        _reg = ffi.NULL
        _id = ffi.NULL
        if isinstance(reg, Register):
            _reg = reg._reg
        elif isinstance(reg, str):
            _dict = self.dictionary
            if not _dict:
                raise ValueError('No dictionary loaded')
            if reg not in _dict.registers(subnode):
                raise_err(lib.IL_REGNOTFOUND, 'Register not found ({})'.format(reg))
            _reg = _dict.registers(subnode)[reg]._reg
        else:
            raise TypeError('Invalid register')
        return _reg, _id

    def raw_read(self, reg, subnode=1):
        """Raw read from servo.

        Args:
            reg (Register): Register.

        Returns:
            int: Otained value

        Raises:
            TypeError: If the register type is not valid.

        """
        return self.read(reg, subnode=subnode)

    def read(self, reg, subnode=1):
        """Read from servo.

        Args:
            reg (str, Register): Register.

        Returns:
            float: Obtained value

        Raises:
            TypeError: If the register type is not valid.

        """
        if isinstance(reg, Register):
            _reg = reg
        elif isinstance(reg, str):
            _dict = self.dictionary
            if not _dict:
                raise ValueError('No dictionary loaded')
            if reg not in _dict.registers(subnode):
                raise_err(lib.IL_REGNOTFOUND, 'Register not found ({})'.format(reg))
            _reg = _dict.registers(subnode)[reg]
        else:
            raise TypeError('Invalid register')

        # Obtain data pointer and function to call
        t, f = self._raw_read[_reg.dtype]
        v = ffi.new(t)

        r = f(self._cffi_servo, _reg._reg, ffi.NULL, v)
        raise_err(r)

        try:
            if self.dictionary:
                _reg = self.dictionary.registers(subnode)[reg]
        except Exception as e:
            pass
        if _reg.dtype == REG_DTYPE.STR:
            value = self.extended_buffer
        else:
            value = v[0]

        if isinstance(value, str):
            value = value.replace('\x00', '')
        return value

    def raw_write(self, reg, data, confirm=True, extended=0, subnode=1):
        """Raw write to servo.

        Args:
            reg (Register): Register.
            data (int): Data.
            confirm (bool, optional): Confirm write.
            extended (int, optional): Extended frame.

        Raises:
            TypeError: If any of the arguments type is not valid or
                unsupported.

        """
        self.write(reg, data, confirm, extended, subnode)

    def write(self, reg, data, confirm=True, extended=0, subnode=1):
        """Write to servo.

        Args:
            reg (Register): Register.
            data (int): Data.
            confirm (bool, optional): Confirm write.
            extended (int, optional): Extended frame.

        Raises:
            TypeError: If any of the arguments type is not valid or
                unsupported.

        """
        if isinstance(reg, Register):
            _reg = reg
        elif isinstance(reg, str):
            _dict = self.dictionary
            if not _dict:
                raise ValueError('No dictionary loaded')
            if reg not in _dict.registers(subnode):
                raise_err(lib.IL_REGNOTFOUND, 'Register not found ({})'.format(reg))
            _reg = _dict.registers(subnode)[reg]
        else:
            raise TypeError('Invalid register')

        # Auto cast floats if register is not float
        if isinstance(data, float) and _reg.dtype != REG_DTYPE.FLOAT:
            data = int(data)

        # Obtain function to call
        f = self._raw_write[_reg.dtype]

        r = f(self._cffi_servo, _reg._reg, ffi.NULL, data, confirm, extended)
        raise_err(r)
    
    def read_sdo(self, idx, subidx, dtype, slave=1):
        """Read SDO from network.

        Args:
            idx (int): Register index.
            subidx (int): Register subindex.
            dtype (REG_DTYPE): Register data type.
            slave (int, Optional): Identifier of an slave in the network.

        Returns:
            float: Obtained value

        Raises:
            TypeError: If the register type is not valid.

        """
        v = ffi.new('double *')
        r = lib.il_net_SDO_read(self._cffi_network, slave, idx, subidx, dtype, v)
        raise_err(r)

        value = v[0]
        return value

    def read_string_sdo(self, idx, subidx, size, slave=1):
        """Read string SDO from network.

        Args:
            idx (int): Register index.
            subidx (int): Register subindex.
            size (int): Size in bytes to read.
            slave (int, Optional): Identifier of an slave in the network.

        Returns:
            str: Obtained value

        Raises:
            TypeError: If the register type is not valid.

        """
        v = ffi.new("char[" + str(size) + "]")
        r = lib.il_net_SDO_read_string(self._cffi_network, slave, idx, subidx, size, v)
        raise_err(r)

        value = pstr(v)
        return value

    def write_sdo(self, idx, subidx, dtype, value, slave=1):
        """Write SDO from network.

        Args:
            idx (int): Register index.
            subidx (int): Register subindex.
            dtype (REG_DTYPE): Register data type.
            value (float): Value to write.
            slave (int, Optional): Identifier of an slave in the network.

        Returns:
            float: Obtained value

        Raises:
            TypeError: If the register type is not valid.

        """
        r = lib.il_net_SDO_write(self._cffi_network, slave, idx, subidx, dtype, value)
        raise_err(r)

    def destroy(self):
        """Destroy servo instance.

        Returns:
            int: Result code.

        """
        r = lib.il_servo_destroy(self._cffi_servo)
        return r

    def reset(self):
        """Reset servo.

        Notes:
            You may need to reconnect the network after reset.

        """
        r = lib.il_servo_reset(self._cffi_servo)
        raise_err(r)

    def get_state(self, subnode=1):
        """Obtain state of the servo.

        Args:
            subnode (int, optional): Subnode.

        Returns:
            tuple: Servo state and state flags.

        """
        state = ffi.new('il_servo_state_t *')
        flags = ffi.new('int *')

        lib.il_servo_state_get(self._cffi_servo, state, flags, subnode)

        return SERVO_STATE(state[0]), flags[0]

    def state_subs_stop(self, stop):
        """Stop servo state subscriptions.

        Args:
            stop (int): start: 0, stop: 1.

        Returns:
            int: Result code.

        """
        return lib.il_servo_state_subs_stop(self._cffi_servo, stop)

    def enable(self, timeout=2., subnode=1):
        """Enable PDS.

        Args:
            timeout (int, float, optional): Timeout (s).
            subnode (int, optional): Subnode.

        """
        r = lib.il_servo_enable(self._cffi_servo, to_ms(timeout), subnode)
        raise_err(r)

    def disable(self, subnode=1):
        """Disable PDS."""
        r = lib.il_servo_disable(self._cffi_servo, subnode)
        raise_err(r)

    def fault_reset(self, subnode=1):
        """Fault reset.

        Args:
            subnode (int, optional): Subnode.

        """
        r = lib.il_servo_fault_reset(self._cffi_servo, subnode)
        raise_err(r)

    def switch_on(self, timeout=2.):
        """Switch on PDS.

        This function switches on the PDS but it does not enable the motor.
        For most application cases, you should only use the `enable`
        function.

        Args:
            timeout (int, float, optional): Timeout (s).

        """
        r = lib.il_servo_switch_on(self._cffi_servo, to_ms(timeout))
        raise_err(r)

    def homing_start(self):
        """Start the homing procedure."""
        r = lib.il_servo_homing_start(self._cffi_servo)
        raise_err(r)

    def homing_wait(self, timeout):
        """Wait until homing completes.

        Notes:
            The homing itself has a configurable timeout. The timeout given
            here is purely a 'communications' timeout, e.g. it could happen
            that the statusword change is never received. This timeout
            should be >= than the programmed homing timeout.

        Args:
            timeout (int, float): Timeout (s).

        """
        r = lib.il_servo_homing_wait(self._cffi_servo, to_ms(timeout))
        raise_err(r)

    def store_parameters(self, subnode=0):
        """Store all the current parameters of the target subnode.

        Args:
            subnode (int): Subnode of the axis.

        Raises:
            ILError: Invalid subnode.
            ILObjectNotExist: Failed to write to the registers.

        """
        if subnode == 0:
            # Store all
            r = 0
            try:
                self.write(reg=STORE_COCO_ALL,
                           data=PASSWORD_STORE_ALL,
                           subnode=subnode)
                logger.info('Store all successfully done.')
            except Exception as e:
                logger.warning('Store all COCO failed. Trying MOCO...')
                r = -1
            if r < 0:
                if self.dictionary.subnodes > SINGLE_AXIS_MINIMUM_SUBNODES:
                    # Multiaxis
                    for dict_subnode in self.dictionary.subnodes:
                        self.write(reg=STORE_MOCO_ALL_REGISTERS[dict_subnode],
                                   data=PASSWORD_STORE_ALL,
                                   subnode=dict_subnode)
                        logger.info('Store axis {} successfully done.'.format(
                            dict_subnode))
                else:
                    # Single axis
                    self.write(reg=STORE_MOCO_ALL_REGISTERS[1],
                               data=PASSWORD_STORE_ALL,
                               subnode=1)
                    logger.info('Store all successfully done.')
        elif subnode > 0:
            # Store axis
            self.write(reg=STORE_MOCO_ALL_REGISTERS[subnode],
                       data=PASSWORD_STORE_ALL,
                       subnode=subnode)
            logger.info('Store axis {} successfully done.'.format(subnode))
        else:
            raise ILError('Invalid subnode.')

    def restore_parameters(self):
        """Restore all the current parameters of all the slave to default.

        Raises:
            ILError: Invalid subnode.
            ILObjectNotExist: Failed to write to the registers.

        """
        self.write(reg=RESTORE_COCO_ALL,
                   data=PASSWORD_RESTORE_ALL,
                   subnode=0)
        logger.info('Restore all successfully done.')

    def is_alive(self):
        """Checks if the servo responds to a reading a register.

        Returns:
            bool: Return code with the result of the read.
        """
        _is_alive = True
        try:
            self.read(STATUS_WORD)
        except ILError as e:
            _is_alive = True
            logger.error(e)
        return _is_alive

    def store_comm(self):
        """Store all servo current communications to the NVM."""
        r = lib.il_servo_store_comm(self._cffi_servo)
        raise_err(r)

    def store_app(self):
        """Store all servo current application parameters to the NVM."""
        r = lib.il_servo_store_app(self._cffi_servo)
        raise_err(r)

    def _dict_load(self, dictionary):
        """Load dictionary.

        Args:
            dictionary (str): Dictionary.

        """
        r = lib.il_servo_dict_load(self._cffi_servo, cstr(dictionary))
        if not hasattr(self, '_errors') or not self._errors:
            self._errors = self._get_all_errors(dictionary)
        raise_err(r)

    def load_configuration(self, dictionary, subnode=0):
        """Load configuration from dictionary file to the servo drive.

        Args:
            dictionary (str): Dictionary.
            subnode (int, optional): Subnode.

        """
        r = lib.il_servo_dict_storage_write(self._cffi_servo, cstr(dictionary),
                                            subnode)
        if not hasattr(self, '_errors') or not self._errors:
            self._errors = self._get_all_errors(dictionary)
        raise_err(r)

    def save_configuration(self, new_path, subnode=0):
        """Read all dictionary registers content and save it to a
            new dictionary.

        Args:
            new_path (str): Dictionary.

        """
        prod_code, rev_number = get_drive_identification(self, subnode)

        r = lib.il_servo_dict_storage_read(self._cffi_servo)
        raise_err(r)

        self.dictionary.save(new_path)

        tree = ET.parse(new_path)
        xml_data = tree.getroot()

        body = xml_data.find('Body')
        device = xml_data.find('Body/Device')
        categories = xml_data.find('Body/Device/Categories')
        errors = xml_data.find('Body/Errors')

        if 'ProductCode' in device.attrib and prod_code is not None:
            device.attrib['ProductCode'] = str(prod_code)
        if 'RevisionNumber' in device.attrib and rev_number is not None:
            device.attrib['RevisionNumber'] = str(rev_number)

        registers_category = xml_data.find('Body/Device/Registers')
        registers = xml_data.findall('Body/Device/Registers/Register')
        if registers_category is None:
            registers_category = xml_data.find(
                'Body/Device/Axes/Axis/Registers')
            registers = xml_data.findall(
                'Body/Device/Axes/Axis/Registers/Register')

        for register in registers:
            if register.attrib['subnode'] != str(
                    subnode) and subnode > 0 and register in registers_category:
                registers_category.remove(register)
            cleanup_register(register)

        device.remove(categories)
        body.remove(errors)

        image = xml_data.find('./DriveImage')
        if image is not None:
            xml_data.remove(image)

        xmlstr = minidom.parseString(ET.tostring(xml_data)).toprettyxml(
            indent="  ", newl='')

        config_file = io.open(new_path, "w", encoding='utf8')
        config_file.write(xmlstr)
        config_file.close()

    def reload_errors(self, dictionary):
        """Force to reload all dictionary errors.

        Args:
            dictionary (str): Dictionary.

        """
        self._errors = self._get_all_errors(dictionary)

    def emcy_subscribe(self, cb):
        """Subscribe to emergency messages.

        Args:
            cb: Callback

        Returns:
            int: Assigned slot.

        """
        cb_handle = ffi.new_handle(cb)

        slot = lib.il_servo_emcy_subscribe(
            self._cffi_servo, lib._on_emcy_cb, cb_handle)
        if slot < 0:
            raise_err(slot)

        self._emcy_cb[slot] = cb_handle

        return slot

    def emcy_unsubscribe(self, slot):
        """Unsubscribe from emergency messages.

        Args:
            slot (int): Assigned slot when subscribed.

        """
        lib.il_servo_emcy_unsubscribe(self._cffi_servo, slot)

        del self._emcy_cb[slot]

    def subscribe_to_status(self, callback):
        """Subscribe to state changes.

        Args:
            callback: Callback

        Returns:
            int: Assigned slot.

        """
        cb_handle = ffi.new_handle(callback)

        slot = lib.il_servo_state_subscribe(
            self._cffi_servo, lib._on_state_change_cb, cb_handle)
        if slot < 0:
            raise_err(slot)

        self._state_cb[slot] = cb_handle

        return slot

    def unsubscribe_from_status(self, slot):
        """Unsubscribe from state changes.

        Args:
            slot (int): Assigned slot when subscribed.

        """
        lib.il_servo_state_unsubscribe(self._cffi_servo, slot)

        del self._state_cb[slot]

    def disturbance_write_data(self, channels, dtypes, data_arr):
        """Write disturbance data.

        Args:
            channels (int or list of int): Channel identifier.
            dtypes (int or list of int): Data type.
            data_arr (list or list of list): Data array.

        """
        if not isinstance(channels, list):
            channels = [channels]
        if not isinstance(dtypes, list):
            dtypes = [dtypes]
        if not isinstance(data_arr[0], list):
            data_arr = [data_arr]
        num_samples = len(data_arr[0])
        self.write(DIST_NUMBER_SAMPLES, num_samples, subnode=0)
        sample_size = 0
        for dtype_val in dtypes:
            sample_size += dtype_size(dtype_val)
        samples_for_write = DIST_FRAME_SIZE // sample_size
        number_writes = num_samples // samples_for_write
        rest_samples = num_samples % samples_for_write
        for i in range(number_writes):
            for index, channel in enumerate(channels):
                self.disturbance_channel_data(
                    channel,
                    dtypes[index],
                    data_arr[index][i * samples_for_write:(i + 1) * samples_for_write])
            self.disturbance_data_size = sample_size * samples_for_write
            self.write(DIST_DATA, sample_size * samples_for_write, False, 1, subnode=0)
        for index, channel in enumerate(channels):
            self.disturbance_channel_data(
                channel,
                dtypes[index],
                data_arr[index][number_writes * samples_for_write:num_samples])
        self.disturbance_data_size = rest_samples * sample_size
        self.write(DIST_DATA, rest_samples * sample_size, False, 1, subnode=0)

    def wait_reached(self, timeout):
        """Wait until the servo does a target reach.

        Args:
            timeout (int, float): Timeout (s).

        """
        r = lib.il_servo_wait_reached(self._cffi_servo, to_ms(timeout))
        raise_err(r)

    def units_update(self):
        """Update units scaling factors.

        Notes:
            This must be called if any encoder parameter, rated torque or
            pole pitch are changed, otherwise, the readings conversions
            will not be correct.

        """
        r = lib.il_servo_units_update(self._cffi_servo)
        raise_err(r)

    def units_factor(self, reg):
        """Obtain units scale factor for the given register.

        Args:
            reg (Register): Register.

        Returns:
            float: Scale factor for the given register.

        """
        return lib.il_servo_units_factor(self._cffi_servo, reg._reg)

    def monitoring_channel_data(self, channel, dtype):
        """Obtain processed monitoring data of a channel.

        Args:
            channel (int): Identity channel number.
            dtype (REG_DTYPE): Data type of the register to map.

        Returns:
            array: Monitoring data.

        """
        data_arr = []
        size = int(self.monitoring_data_size)
        bytes_per_block = self.monitoring_get_bytes_per_block()
        if dtype == REG_DTYPE.U16:
            data_arr = lib.il_net_monitoring_channel_u16(self._cffi_network, channel)
        elif dtype == REG_DTYPE.S16:
            data_arr = lib.il_net_monitoring_channel_s16(self._cffi_network, channel)
        elif dtype == REG_DTYPE.U32:
            data_arr = lib.il_net_monitoring_channel_u32(self._cffi_network, channel)
        elif dtype == REG_DTYPE.S32:
            data_arr = lib.il_net_monitoring_channel_s32(self._cffi_network, channel)
        elif dtype == REG_DTYPE.FLOAT:
            data_arr = lib.il_net_monitoring_channel_flt(self._cffi_network, channel)
        ret_arr = []
        for i in range(0, int(size / bytes_per_block)):
            ret_arr.append(data_arr[i])
        return ret_arr

    def monitoring_remove_all_mapped_registers(self):
        """Remove all monitoring mapped registers.

        Returns:
            int: Result code.

        """
        return lib.il_net_remove_all_mapped_registers(self._cffi_network)

    def monitoring_set_mapped_register(self, channel, reg_idx, dtype):
        """Set monitoring mapped register.

        Args:
            channel (int): Identity channel number.
            reg_idx (int): Register address to map.
            dtype (REG_DTYPE): Data type of the register to map.

        Returns:
            int: Result code.

        """
        return lib.il_net_set_mapped_register(self._cffi_network, channel,
                                              reg_idx, dtype)

    def monitoring_get_num_mapped_registers(self):
        """Obtain the number of mapped registers.

        Returns:
            int: Actual number of mapped registers.

        """
        return lib.il_net_num_mapped_registers_get(self._cffi_network)

    def monitoring_enable(self):
        """Enable monitoring process.

        Returns:
            int: Result code.

        """
        return lib.il_net_enable_monitoring(self._cffi_network)

    def monitoring_disable(self):
        """Disable monitoring process.

        Returns:
            int: Result code.

        """
        return lib.il_net_disable_monitoring(self._cffi_network)

    def monitoring_read_data(self):
        """Obtain processed monitoring data.

        Returns:
            array: Actual processed monitoring data.

        """
        return lib.il_net_read_monitoring_data(self._cffi_network)

    def monitoring_get_bytes_per_block(self):
        """Obtain Bytes x Block configured.

        Returns:
            int: Actual number of Bytes x Block configured.

        """
        return lib.il_net_monitornig_bytes_per_block_get(self._cffi_network)

    def disturbance_channel_data(self, channel, dtype, data_arr):
        """Send disturbance data.

        Args:
            channel (int): Identity channel number.
            dtype (REG_DTYPE): Data type of the register mapped.
            data_arr (array): Data that will be sent to the drive.

        Returns:
            int: Return code.

        """
        if dtype == REG_DTYPE.U16:
            lib.il_net_disturbance_data_u16_set(self._cffi_network, channel, data_arr)
        elif dtype == REG_DTYPE.S16:
            lib.il_net_disturbance_data_s16_set(self._cffi_network, channel, data_arr)
        elif dtype == REG_DTYPE.U32:
            lib.il_net_disturbance_data_u32_set(self._cffi_network, channel, data_arr)
        elif dtype == REG_DTYPE.S32:
            lib.il_net_disturbance_data_s32_set(self._cffi_network, channel, data_arr)
        elif dtype == REG_DTYPE.FLOAT:
            lib.il_net_disturbance_data_flt_set(self._cffi_network, channel, data_arr)
        return 0

    def disturbance_remove_all_mapped_registers(self):
        """Remove all disturbance mapped registers.

        Returns:
            int: Return code.

        """
        return lib.il_net_disturbance_remove_all_mapped_registers(self._cffi_network)

    def disturbance_set_mapped_register(self, channel, address, dtype):
        """Set disturbance mapped register.

        Args:
            channel (int): Identity channel number.
            address (int): Register address to map.
            dtype (REG_DTYPE): Data type of the register to map.

        Returns:
            int: Return code.

        """
        return lib.il_net_disturbance_set_mapped_register(self._cffi_network, channel,
                                                          address, dtype)

    @property
    def dictionary(self):
        """Obtain dictionary of the servo."""
        _dict = lib.il_servo_dict_get(self._cffi_servo)

        return IPBDictionary._from_dict(_dict) if _dict else None

    @property
    def info(self):
        """Obtain servo information.

        Returns:
            dict: Servo information.

        """
        _info = ffi.new('il_servo_info_t *')

        r = lib.il_servo_info_get(self._cffi_servo, _info)
        raise_err(r)

        PRODUCT_ID_REG = IPBRegister(identifier='', address=0x06E1,
                                     dtype=REG_DTYPE.U32,
                                     access=REG_ACCESS.RO, cyclic='CONFIG',
                                     units='0')

        product_id = self.read(PRODUCT_ID_REG)

        return {'serial': _info.serial,
                'name': pstr(_info.name),
                'sw_version': pstr(_info.sw_version),
                'hw_variant': pstr(_info.hw_variant),
                'prod_code': product_id,
                'revision': _info.revision}

    @property
    def units_torque(self):
        """SERVO_UNITS_TORQUE: Torque units."""
        return SERVO_UNITS_TORQUE(lib.il_servo_units_torque_get(self._cffi_servo))

    @units_torque.setter
    def units_torque(self, units):
        lib.il_servo_units_torque_set(self._cffi_servo, units.value)

    @property
    def units_pos(self):
        """SERVO_UNITS_POS: Position units."""
        return SERVO_UNITS_POS(lib.il_servo_units_pos_get(self._cffi_servo))

    @units_pos.setter
    def units_pos(self, units):
        lib.il_servo_units_pos_set(self._cffi_servo, units.value)

    @property
    def units_vel(self):
        """SERVO_UNITS_VEL: Velocity units."""
        return SERVO_UNITS_VEL(lib.il_servo_units_vel_get(self._cffi_servo))

    @units_vel.setter
    def units_vel(self, units):
        lib.il_servo_units_vel_set(self._cffi_servo, units.value)

    @property
    def units_acc(self):
        """SERVO_UNITS_ACC: Acceleration units."""
        return SERVO_UNITS_ACC(lib.il_servo_units_acc_get(self._cffi_servo))

    @units_acc.setter
    def units_acc(self, units):
        lib.il_servo_units_acc_set(self._cffi_servo, units.value)

    @property
    def mode(self):
        """Obtains Operation mode.

        Returns:
            SERVO_MODE: Current operation mode.

        """
        mode = ffi.new('il_servo_mode_t *')

        r = lib.il_servo_mode_get(self._cffi_servo, mode)
        raise_err(r)

        return SERVO_MODE(mode[0])

    @mode.setter
    def mode(self, mode):
        """Set Operation mode.

        Args:
            mode (SERVO_MODE): Operation mode.

        """
        r = lib.il_servo_mode_set(self._cffi_servo, mode.value)
        raise_err(r)

    @property
    def errors(self):
        """Obtain drive errors.

        Returns:
            dict: Current errors.

        """
        return self._errors

    @property
    def subnodes(self):
        """Obtain number of subnodes.

        Returns:
            int: Current number of subnodes.

        """
        return int(ffi.cast('int', lib.il_servo_subnodes_get(self._cffi_servo)))

    @property
    def ol_voltage(self):
        """Get open loop voltage.

        Returns:
            float: Open loop voltage (% relative to DC-bus, -1...1).

        """
        voltage = ffi.new('double *')
        r = lib.il_servo_ol_voltage_get(self._cffi_servo, voltage)
        raise_err(r)

        return voltage[0]

    @ol_voltage.setter
    def ol_voltage(self, voltage):
        """Set the open loop voltage (% relative to DC-bus, -1...1).

        Args:
            float: Open loop voltage.

        """
        r = lib.il_servo_ol_voltage_set(self._cffi_servo, voltage)
        raise_err(r)

    @property
    def ol_frequency(self):
        """Get open loop frequency.

        Returns:
            float: Open loop frequency (mHz).

        """
        frequency = ffi.new('double *')
        r = lib.il_servo_ol_frequency_get(self._cffi_servo, frequency)
        raise_err(r)

        return frequency[0]

    @ol_frequency.setter
    def ol_frequency(self, frequency):
        """Set the open loop frequency (mHz).

        Args:
            float: Open loop frequency.

        """
        r = lib.il_servo_ol_frequency_set(self._cffi_servo, frequency)
        raise_err(r)

    @property
    def torque(self):
        """Get actual torque.

        Returns:
            float: Actual torque.

        """
        torque = ffi.new('double *')
        r = lib.il_servo_torque_get(self._cffi_servo, torque)
        raise_err(r)

        return torque[0]

    @torque.setter
    def torque(self, torque):
        """Set the target torque.

        Args:
            float: Target torque.

        """
        r = lib.il_servo_torque_set(self._cffi_servo, torque)
        raise_err(r)

    @property
    def position(self):
        """Get actual position.

        Returns:
            float: Actual position.

        """
        position = ffi.new('double *')
        r = lib.il_servo_position_get(self._cffi_servo, position)
        raise_err(r)

        return position[0]

    @position.setter
    def position(self, pos):
        """Set the target position.

        Notes:
            Position can be either a single position, or a tuple/list
            containing in the first position the position, and in the
            second a dictionary with the following options:

                - immediate (bool): If True, the servo will go to the
                  position immediately, otherwise it will push the position
                  to the buffer. Defaults to True.
                - relative (bool): If True, the position will be taken as
                  relative, otherwise it will be taken as absolute.
                  Defaults to False.
                - sp_timeout (int, float): Set-point acknowledge
                  timeout (s).

        Args:
            pos (float): Target position.

        """
        immediate = 1
        relative = 0
        sp_timeout = lib.IL_SERVO_SP_TIMEOUT_DEF

        if isinstance(pos, (tuple, list)):
            if len(pos) != 2 or not isinstance(pos[1], dict):
                raise TypeError('Unexpected position')

            if 'immediate' in pos[1]:
                immediate = int(pos[1]['immediate'])

            if 'relative' in pos[1]:
                relative = int(pos[1]['relative'])

            if 'sp_timeout' in pos[1]:
                sp_timeout = to_ms(pos[1]['sp_timeout'])

            pos = pos[0]

        r = lib.il_servo_position_set(self._cffi_servo, pos, immediate, relative,
                                      sp_timeout)
        raise_err(r)

    @property
    def position_res(self):
        """Get position resolution.

        Returns:
            int: Position resolution (c/rev/s, c/ppitch/s).

        """
        res = ffi.new('uint32_t *')
        r = lib.il_servo_position_res_get(self._cffi_servo, res)
        raise_err(r)

        return res[0]

    @property
    def velocity(self):
        """Get actual velocity.

        Returns:
            float: Actual velocity.

        """
        velocity = ffi.new('double *')
        r = lib.il_servo_velocity_get(self._cffi_servo, velocity)
        raise_err(r)

        return velocity[0]

    @velocity.setter
    def velocity(self, velocity):
        """Set the target velocity.

        Args:
            velocity (float): Target velocity.

        """
        r = lib.il_servo_velocity_set(self._cffi_servo, velocity)
        raise_err(r)

    @property
    def velocity_res(self):
        """Get velocity resolution.

        Returns:
            int: Velocity resolution (c/rev, c/ppitch).

        """
        res = ffi.new('uint32_t *')
        r = lib.il_servo_velocity_res_get(self._cffi_servo, res)
        raise_err(r)

        return res[0]
    
    @property
    def monitoring_data(self):
        """Obtain monitoring data.

        Returns:
            array: Current monitoring data.

        """
        monitoring_data = lib.il_net_monitornig_data_get(self._cffi_network)
        size = int(self.monitoring_data_size / 2)
        ret_arr = []
        for i in range(0, size):
            ret_arr.append(monitoring_data[i])
        return ret_arr

    @property
    def monitoring_data_size(self):
        """Obtain monitoring data size.

        Returns:
            int: Current monitoring data size.

        """
        return lib.il_net_monitornig_data_size_get(self._cffi_network)

    @property
    def disturbance_data(self):
        """Obtain disturbance data.

        Returns:
            array: Current disturbance data.

        """
        disturbance_data = lib.il_net_disturbance_data_get(self._cffi_network)
        size = int(self.disturbance_data_size / 2)
        ret_arr = []
        for i in range(0, size):
            ret_arr.append(disturbance_data[i])
        return ret_arr

    @disturbance_data.setter
    def disturbance_data(self, value):
        """Set disturbance data.

        Args:
            value (array): Array with the disturbance to send.

        """
        disturbance_arr = value
        disturbance_arr = \
            np.pad(disturbance_arr,
                   (0, int(self.disturbance_data_size / 2) - len(value)),
                   'constant')
        lib.il_net_disturbance_data_set(self._cffi_network, disturbance_arr.tolist())

    @property
    def disturbance_data_size(self):
        """Obtain disturbance data size.

        Returns:
            int: Current disturbance data size.

        """
        return lib.il_net_disturbance_data_size_get(self._cffi_network)

    @disturbance_data_size.setter
    def disturbance_data_size(self, value):
        """Set disturbance data size.

        Args:
            value (int): Disturbance data size in bytes.

        """
        lib.il_net_disturbance_data_size_set(self._cffi_network, value)

    @property
    def extended_buffer(self):
        """Obtain extended buffer data.

        Returns:
            str: Current extended buffer data.

        """
        ext_buff = lib.il_net_extended_buffer_get(self._cffi_network)
        return pstr(ext_buff)

