from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

from math import pi
from copy import deepcopy

from compas.data import Data

# These joint types known to configuration are a match
# to the ones defined in `Joint` class, but we redefine here
# to avoid a circular dependency
_JOINT_REVOLUTE = 0
_JOINT_CONTINUOUS = 1
_JOINT_PRISMATIC = 2
_JOINT_PLANAR = 5


def joint_names_validator(joint_names, key=None, value=None):
    new_joint_names = list(joint_names)
    if key is not None and value is not None:
        new_joint_names.__setitem__(key, value)
    if len(new_joint_names) != len(set(new_joint_names)):
        raise ValueError("joint_names cannot have repeated values.")


class FixedLengthList(list):
    """Restriction of the standard Python list to prevent changes in length
    while maintaining the ability to change values.  An optional ``validator``
    can be passed to the constructor which would be used to validate changes
    to the values.

    Parameters
    ----------
    validator : :obj:`function`
        A boolean function with the same signature as __setiem__, ie
        validator(fll: FixedLengthList, key: slice, value: Any) -> bool
    """

    def __init__(self, *args, **kwargs):
        super(FixedLengthList, self).__init__(*args)
        self.validator = kwargs.get("validator")
        if self.validator:
            self.validator(self)

    def __setitem__(self, key, value):
        # included to obstruct the all too common `l[1:1] = range(100)`-type usage
        value_length = len(value) if hasattr(value, "__len__") else 1
        slice_length = len(range(*key.indices(self.__len__()))) if isinstance(key, slice) else 1
        if slice_length != value_length:
            raise TypeError("Cannot change length of FixedLengthList")
        if self.validator:
            self.validator(self, key, value)
        super(FixedLengthList, self).__setitem__(key, value)

    def __setslice__(self, i, j, sequence):
        # for ironpython
        slice_end = min(j, self.__len__())
        slice_length = slice_end - i
        value_length = len(sequence)
        if slice_length != value_length:
            raise TypeError("Cannot change length of FixedLengthList")
        if self.validator:
            self.validator(self, slice(i, j), sequence)
        super(FixedLengthList, self).__setslice__(i, j, sequence)

    def __copy__(self):
        return FixedLengthList(self)

    def __deepcopy__(self, memodict={}):
        return FixedLengthList([deepcopy(value, memodict) for value in self])

    def append(self, item):
        raise TypeError("Cannot change length of FixedLengthList")

    def extend(self, other):
        raise TypeError("Cannot change length of FixedLengthList")

    def insert(self, i, item):
        raise TypeError("Cannot change length of FixedLengthList")

    def remove(self, item):
        raise TypeError("Cannot change length of FixedLengthList")

    def pop(self, i=-1):
        raise TypeError("Cannot change length of FixedLengthList")

    def clear(self):
        raise TypeError("Cannot change length of FixedLengthList")


class Configuration(Data):
    """Represents the configuration of a robot based on the state of its joints.
    If the names of joints are also provided, the configuration behaves as a
    dictionary of joint name-value pairs.

    This concept is also refered to as \"Joint State\".

    Parameters
    ----------
    joint_values : list of float
        Joint values expressed in radians or meters, depending on the respective
        type.
    joint_types : list of :attr:`compas.robots.Joint.SUPPORTED_TYPES`
        Joint types, e.g. a list of :attr:`compas.robots.Joint.REVOLUTE` for
        revolute joints.
    joint_names : list[str], optional
        List of joint names.

    Attributes
    ----------
    joint_values : list of float
        Joint values expressed in radians or meters, depending on the respective
        type.
    joint_types : list of :attr:`compas.robots.Joint.SUPPORTED_TYPES`
        Joint types, e.g. a list of :attr:`compas.robots.Joint.REVOLUTE` for
        revolute joints.
    joint_names : list[str]
        List of joint names.
    data : dict
        The data representing the configuration.
    prismatic_values : list of float
        Prismatic joint values in meters.
    revolute_values : list of float
        Revolute joint values in radians.

    Examples
    --------
    >>> config = Configuration.from_revolute_values([math.pi/2, 0., 0.])
    >>> config.joint_values
    [1.5707963267948966, 0.0, 0.0]

    >>> config = Configuration.from_prismatic_and_revolute_values([8.312], [math.pi/2, 0., 0., 0., 2*math.pi, 0.8])
    >>> str(config)
    'Configuration((8.312, 1.571, 0.000, 0.000, 0.000, 6.283, 0.800), (2, 0, 0, 0, 0, 0, 0))'

    >>> from compas.robots import Joint
    >>> config = Configuration([math.pi/2, 3., 0.1], [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR])
    >>> str(config)
    'Configuration((1.571, 3.000, 0.100), (0, 2, 5))'

    """

    def __init__(self, joint_values=None, joint_types=None, joint_names=None, **kwargs):
        super(Configuration, self).__init__(**kwargs)

        joint_values = FixedLengthList(joint_values or [])
        joint_types = FixedLengthList(joint_types or [])
        joint_names = FixedLengthList(joint_names or [], validator=joint_names_validator)

        if len(joint_values) != len(joint_types):
            raise ValueError(
                "{} joint_values must have {} joint_types, but {} given.".format(
                    len(joint_values), len(joint_values), len(joint_types)
                )
            )

        if joint_names and len(joint_names) != len(joint_values):
            raise ValueError(
                "{} joint_values must have either 0 or {} names, but {} given".format(
                    len(joint_values), len(joint_values), len(joint_names)
                )
            )

        self._precision = "3f"
        self._joint_values = joint_values
        self._joint_types = joint_types
        self._joint_names = joint_names

    @property
    def joint_values(self):
        """Joint values expressed in radians or meters, depending on the respective
        type.

        Returns
        -------
        list of float
        """
        return self._joint_values

    @joint_values.setter
    def joint_values(self, values):
        if len(self._joint_values) != len(values):
            raise Exception(
                "joint_values must have length {}, object of length {} given".format(
                    len(self._joint_values), len(values)
                )
            )
        self._joint_values = FixedLengthList(values)

    @property
    def joint_types(self):
        """Joint joint_types, e.g. a list of :attr:`compas.robots.Joint.REVOLUTE` for
        revolute joints.

        Returns
        -------
        list of :attr:`compas.robots.Joint.SUPPORTED_TYPES`
        """
        return self._joint_types

    @joint_types.setter
    def joint_types(self, joint_types):
        if len(self._joint_types) != len(joint_types):
            raise Exception(
                "joint_types must have length {}, object of length {} given".format(
                    len(self._joint_types), len(joint_types)
                )
            )
        self._joint_types = FixedLengthList(joint_types)

    @property
    def joint_names(self):
        """ "List of joint names.

        Returns
        -------
        list[str]
        """
        return self._joint_names

    @joint_names.setter
    def joint_names(self, names):
        if names and len(self._joint_values) != len(names):
            raise ValueError(
                "joint_types must have length {}, object of length {} given".format(len(self._joint_values), len(names))
            )
        self._joint_names = FixedLengthList(names, validator=joint_names_validator)

    def __str__(self):
        """Return a human-readable string representation of the instance."""
        v_str = ("(" + ", ".join(["%." + self._precision] * len(self.joint_values)) + ")") % tuple(self.joint_values)
        if len(self.joint_names):
            return "Configuration({}, {}, {})".format(v_str, tuple(self.joint_types), tuple(self.joint_names))
        else:
            return "Configuration({}, {})".format(v_str, tuple(self.joint_types))

    def __repr__(self):
        """Printable representation of :class:`Configuration`."""
        return self.__str__()

    def __getitem__(self, item):
        for name, value in zip(self.joint_names, self.joint_values):
            if name == item:
                return value
        raise KeyError(item)

    def __setitem__(self, item, value):
        i = self.joint_names.index(item)
        if i < 0:
            raise KeyError(item)
        self.joint_values[i] = value

    def __bool__(self):
        # If __bool__ is not overwritten, then calls to bool default to the value given by __len__.
        # Since __len__ should reflect the number of items given by iterating over the object,
        # __len__ returns len(self.joint_names).  However, there are valid Configurations that lack
        # joint names, and should not be falsy.
        return True

    def __nonzero__(self):
        # ironpython's version of __bool__
        return self.__bool__()

    def __len__(self):
        return len(self.joint_names)

    def __iter__(self):
        return iter(self.joint_names)

    def items(self):
        return zip(self.joint_names, self.joint_values)

    def keys(self):
        return iter(self.joint_names)

    def values(self):
        return iter(self.joint_values)

    def get(self, key, default=None):
        try:
            value = self[key]
        except KeyError:
            value = default
        return value

    @classmethod
    def from_revolute_values(cls, values, joint_names=None):
        """Construct a configuration from revolute joint values in radians.

        Parameters
        ----------
        values : list of float
            Joint values expressed in radians.
        joint_names : list[str], optional
            List of joint names.

        Returns
        -------
        :class:`Configuration`
             An instance of :class:`Configuration` instance.
        """
        values = list(values)
        joint_names = list(joint_names or [])
        return cls.from_data(
            {
                "joint_values": values,
                "joint_types": [_JOINT_REVOLUTE] * len(values),
                "joint_names": joint_names,
            }
        )

    @classmethod
    def from_prismatic_and_revolute_values(cls, prismatic_values, revolute_values, joint_names=None):
        """Construct a configuration from prismatic and revolute joint values.

        Parameters
        ----------
        prismatic_values : list of float
            Positions on the external axis system in meters.
        revolute_values : list of float
            Joint values expressed in radians.
        joint_names : list[str], optional
            List of joint names.

        Returns
        -------
        :class:`Configuration`
             An instance of :class:`Configuration`.
        """
        # Force iterables into lists
        prismatic_values = list(prismatic_values)
        revolute_values = list(revolute_values)
        joint_names = list(joint_names or [])
        values = prismatic_values + revolute_values
        joint_types = [_JOINT_PRISMATIC] * len(prismatic_values) + [_JOINT_REVOLUTE] * len(revolute_values)
        return cls.from_data(
            {
                "joint_values": values,
                "joint_types": joint_types,
                "joint_names": joint_names,
            }
        )

    @property
    def data(self):
        """dict : The data representing the configuration.

        By assigning a data dictionary to this property, the current data of
        the configuration will be replaced by the data in the dict. The
        data getter and setter should always be used in combination with each
        other.
        """
        return {
            "joint_values": self.joint_values,
            "joint_types": self.joint_types,
            "joint_names": self.joint_names,
        }

    @data.setter
    def data(self, data):
        self._joint_values = FixedLengthList(data.get("joint_values") or data.get("values") or [])
        self._joint_types = FixedLengthList(data.get("joint_types") or data.get("types") or [])
        self._joint_names = FixedLengthList(data.get("joint_names") or [])

    @property
    def prismatic_values(self):
        """list of float : Prismatic joint values in meters.

        E.g. positions on the external axis system.
        """
        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == _JOINT_PRISMATIC]

    @property
    def revolute_values(self):
        """list of float : Revolute joint values in radians."""
        return [v for i, v in enumerate(self.joint_values) if self.joint_types[i] == _JOINT_REVOLUTE]

    def scale(self, scale_factor):
        """Scales the joint positions of the current configuration.

        Only scalable joints are scaled, i.e. planar and prismatic joints.

        Parameters
        ----------
        scale_factor : float
            Scale factor.

        Returns
        -------
        None
        """
        values_scaled = []

        for value, joint_type in zip(self.joint_values, self.joint_types):
            if joint_type in (_JOINT_PLANAR, _JOINT_PRISMATIC):
                value *= scale_factor
            values_scaled.append(value)

        self._joint_values = FixedLengthList(values_scaled)

    def scaled(self, scale_factor):
        """Return a scaled copy of this configuration.

        Only scalable joints are scaled, i.e. planar and prismatic joints.

        Parameters
        ----------
        scale_factor : float
            Scale factor

        Returns
        -------
        None
        """
        config = self.copy()
        config.scale(scale_factor)
        return config

    def iter_differences(self, other):
        """Generator over the differences to another `Configuration`'s joint_values.

        If the joint type is revolute or continuous, the smaller difference
        (+/- 2*:math:`\\pi`) is calculated.

        Parameters
        ----------
        other : :class:`Configuration`
            The configuration to compare to.

        Yields
        ------
        float
            The next difference to the `Configuration`'s joint_values.

        Raises
        ------
        ValueError
            If the configurations are not comparable.

        Examples
        --------
        >>> from compas.geometry import allclose
        >>> c1 = Configuration.from_revolute_values([1, 0, 3])
        >>> c2 = Configuration.from_revolute_values([1, 2 * pi, 4])
        >>> allclose(c1.iter_differences(c2), [0.0, 0.0, -1.0])
        True
        >>> c1 = Configuration.from_revolute_values([1, 0, 3])
        >>> c2 = Configuration.from_revolute_values([1, -2 * pi - 0.2, 4])
        >>> allclose(c1.iter_differences(c2), [0.0, 0.2, -1.0])
        True
        """
        if self.joint_names and other.joint_names:
            if set(self.joint_names) != set(other.joint_names):
                raise ValueError("Configurations have different joint names.")
            other_value_by_name = dict(zip(other.joint_names, other.joint_values))
            sorted_other_values = [other_value_by_name[name] for name in self.joint_names]
            value_pairs = zip(self.joint_values, sorted_other_values)
        else:
            if len(self.joint_values) != len(other.joint_values):
                raise ValueError("Can't compare configurations with different lengths of joint_values.")
            value_pairs = zip(self.joint_values, other.joint_values)

        for i, (v1, v2) in enumerate(value_pairs):
            diff = v1 - v2
            if self.joint_types[i] in [_JOINT_REVOLUTE, _JOINT_CONTINUOUS]:
                d1 = diff % (2 * pi)
                d1 = d1 if diff >= 0 else d1 - 2 * pi
                d2 = d1 - 2 * pi if diff >= 0 else d1 + 2 * pi
                diff = d1 if abs(d1) < abs(d2) else d2
            yield diff

    def max_difference(self, other):
        """Returns the maximum difference to another `Configuration`'s joint values.

        Parameters
        ----------
        other : :class:`Configuration`
            The configuration to compare to.

        Returns
        -------
        float
            The maximum absolute difference.

        Examples
        --------
        >>> c1 = Configuration.from_revolute_values([1, 0, 3])
        >>> c2 = Configuration.from_revolute_values([1, 2 * pi, 4])
        >>> c1.max_difference(c2)
        1.0
        """
        return max([abs(v) for v in self.iter_differences(other)])

    def close_to(self, other, tol=1e-3):
        """Returns ``True`` if the other `Configuration`'s joint_values are within a certain range.

        Parameters
        ----------
        other : :class:`Configuration`
            The configuration to compare to.
        tol : float
            The tolerance under which we consider 2 floats the same. Defaults to 1e-3.

        Returns
        -------
        :obj:`bool`
            ``True`` if the other `Configuration`'s joint_values are within a certain
            tolerance, `False` otherwise.

        Examples
        --------
        >>> c1 = Configuration.from_revolute_values([1, 0, 3])
        >>> c2 = Configuration.from_revolute_values([1, 2 * pi, 3])
        >>> c1.close_to(c2)
        True
        """
        for diff in self.iter_differences(other):
            if abs(diff) > tol:
                return False
        return True

    @property
    def has_joint_names(self):
        """Returns ``True`` when there is a joint name for every value."""
        return len(self.joint_values) == len(self.joint_names)

    def check_joint_names(self):
        """Raises an error if there is not a joint name for every value."""
        if not self.has_joint_names:
            if not len(self.joint_names):
                raise ValueError("Joint names are required for this operation.")
            else:
                raise ValueError(
                    "Joint names do not match the number of joint values. Joint names={}, Joint values={}".format(
                        len(self.joint_values), len(self.joint_names)
                    )
                )

    @property
    def joint_dict(self):
        """A dictionary of joint values by joint name."""
        self.check_joint_names()
        return dict(zip(self.joint_names, self.joint_values))

    @property
    def type_dict(self):
        """A dictionary of joint types by joint name."""
        self.check_joint_names()
        return dict(zip(self.joint_names, self.joint_types))

    def merge(self, other):
        """Merge the configuration with another configuration in place along joint names.
        The other configuration takes precedence over this configuration in
        case a joint value is present in both.

        Notes
        -----
        Caution: ``joint_names`` may be rearranged.

        Parameters
        ----------
        other : :class:`Configuration`
            The configuration to be merged.

        Raises
        ------
        :exc:`ValueError`
            If the configuration or the ``other`` configuration does not specify
            joint names for all joint values.
        """
        _joint_dict = self.joint_dict
        _joint_dict.update(other.joint_dict)

        _type_dict = self.type_dict
        _type_dict.update(other.type_dict)

        self.joint_names = list(_joint_dict.keys())
        self.joint_values = [_joint_dict[name] for name in self.joint_names]
        self.joint_types = [_type_dict[name] for name in self.joint_names]

    def merged(self, other):
        """Get a new ``Configuration`` with this configuration merged with another configuration.
        The other configuration takes precedence over this configuration in
        case a joint value is present in both.

        Notes
        -----
        Caution: ``joint_names`` may be rearranged.

        Parameters
        ----------
        other : :class:`Configuration`
            The configuration to be merged.

        Returns
        -------
        :class:`Configuration`
            A configuration with values for all included joints.

        Raises
        ------
        :exc:`ValueError`
            If the configuration or the ``other`` configuration does not specify
            joint names for all joint values.
        """
        _joint_dict = self.joint_dict
        _joint_dict.update(other.joint_dict)

        _type_dict = self.type_dict
        _type_dict.update(other.type_dict)

        joint_names = list(_joint_dict.keys())
        joint_values = [_joint_dict[name] for name in joint_names]
        joint_types = [_type_dict[name] for name in joint_names]

        return Configuration(joint_values, joint_types, joint_names)
