import json
import math

import tf


class Pose:
    def __init__(self, position, orientation):
        self.position = position
        self.orientation = orientation

    @classmethod
    def from_json(cls, path):
        with open(path) as f:
            transform = json.load(f)

        # The NxLib transformation in the JSON file is in millimeters. We
        # represent it in meters like everything in ROS.
        position = [transform["Translation"][0] / 1000,
                    transform["Translation"][1] / 1000,
                    transform["Translation"][2] / 1000]
        axis = (transform["Rotation"]["Axis"][0], transform["Rotation"]["Axis"][1], transform["Rotation"]["Axis"][2])
        orientation = tf.transformations.quaternion_about_axis(transform["Rotation"]["Angle"], axis)

        return cls(position, orientation)

    @classmethod
    def from_message(cls, message):
        position = [message.position.x,
                    message.position.y,
                    message.position.z]
        orientation = [message.orientation.x,
                       message.orientation.y,
                       message.orientation.z,
                       message.orientation.w]

        return cls(position, orientation)

    def inverse(self):
        """
        Get the inverse of the pose.
        """
        translation = tf.transformations.translation_matrix(self.position)
        rotation = tf.transformations.quaternion_matrix(self.orientation)
        transform = tf.transformations.concatenate_matrices(translation, rotation)

        inverse_transform = tf.transformations.inverse_matrix(transform)
        translation = tf.transformations.translation_from_matrix(inverse_transform)
        rotation = tf.transformations.quaternion_from_matrix(inverse_transform)

        return Pose(translation, rotation)

    def equals(self, other, tolerance=0.001):
        """
        Check whether this pose is equal to another one. The poses are
        considered equal, when all of the entries are within the given tolerance
        of each other.
        """
        for a, b in zip(self.position, other.position):
            if abs(a - b) > tolerance:
                return False
        for a, b in zip(self.orientation, other.orientation):
            if abs(a - b) > tolerance:
                return False
        return True


class ImagePoint:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    @classmethod
    def from_message(cls, message):
        return cls(message.x, message.y)

    def distance_to(self, other):
        return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)

    def equals(self, other, tolerance=1):
        """
        Check whether the points are the same, up to the given distance.
        """
        return self.distance_to(other) < tolerance


class RegionOfInterest:
    def __init__(self, lower, upper):
        self.lower = lower
        self.upper = upper

    @classmethod
    def from_message(cls, message):
        lower = [message.lower.x, message.lower.y, message.lower.z]
        upper = [message.upper.x, message.upper.y, message.upper.z]

        return cls(lower, upper)

    def write_to_message(self, message):
        message.lower.x = self.lower[0]
        message.lower.y = self.lower[1]
        message.lower.z = self.lower[2]

        message.upper.x = self.upper[0]
        message.upper.y = self.upper[1]
        message.upper.z = self.upper[2]

    def is_empty(self):
        for l, u in zip(self.lower, self.upper):
            if l >= u:
                return True
        return False

    def equals(self, other, tolerance=0):
        for a, b in zip(self.lower, other.lower):
            if abs(a - b) > tolerance:
                return False
        for a, b in zip(self.upper, other.upper):
            if abs(a - b) > tolerance:
                return False
        return True

import json
import math

import tf


class Pose:
    def __init__(self, position, orientation):
        self.position = position
        self.orientation = orientation

    @classmethod
    def from_json(cls, path):
        with open(path) as f:
            transform = json.load(f)

        # The NxLib transformation in the JSON file is in millimeters. We
        # represent it in meters like everything in ROS.
        position = [transform["Translation"][0] / 1000,
                    transform["Translation"][1] / 1000,
                    transform["Translation"][2] / 1000]
        axis = (transform["Rotation"]["Axis"][0], transform["Rotation"]["Axis"][1], transform["Rotation"]["Axis"][2])
        orientation = tf.transformations.quaternion_about_axis(transform["Rotation"]["Angle"], axis)

        return cls(position, orientation)

    @classmethod
    def from_message(cls, message):
        position = [message.position.x,
                    message.position.y,
                    message.position.z]
        orientation = [message.orientation.x,
                       message.orientation.y,
                       message.orientation.z,
                       message.orientation.w]

        return cls(position, orientation)

    def inverse(self):
        """
        Get the inverse of the pose.
        """
        translation = tf.transformations.translation_matrix(self.position)
        rotation = tf.transformations.quaternion_matrix(self.orientation)
        transform = tf.transformations.concatenate_matrices(translation, rotation)

        inverse_transform = tf.transformations.inverse_matrix(transform)
        translation = tf.transformations.translation_from_matrix(inverse_transform)
        rotation = tf.transformations.quaternion_from_matrix(inverse_transform)

        return Pose(translation, rotation)

    def equals(self, other, tolerance=0.001):
        """
        Check whether this pose is equal to another one. The poses are
        considered equal, when all of the entries are within the given tolerance
        of each other.
        """
        for a, b in zip(self.position, other.position):
            if abs(a - b) > tolerance:
                return False
        for a, b in zip(self.orientation, other.orientation):
            if abs(a - b) > tolerance:
                return False
        return True


class ImagePoint:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    @classmethod
    def from_message(cls, message):
        return cls(message.x, message.y)

    def distance_to(self, other):
        return math.sqrt((self.x - other.x) ** 2 + (self.y - other.y) ** 2)

    def equals(self, other, tolerance=1):
        """
        Check whether the points are the same, up to the given distance.
        """
        return self.distance_to(other) < tolerance


class RegionOfInterest:
    def __init__(self, lower, upper):
        self.lower = lower
        self.upper = upper

    @classmethod
    def from_message(cls, message):
        lower = [message.lower.x, message.lower.y, message.lower.z]
        upper = [message.upper.x, message.upper.y, message.upper.z]

        return cls(lower, upper)

    def write_to_message(self, message):
        message.lower.x = self.lower[0]
        message.lower.y = self.lower[1]
        message.lower.z = self.lower[2]

        message.upper.x = self.upper[0]
        message.upper.y = self.upper[1]
        message.upper.z = self.upper[2]

    def is_empty(self):
        for l, u in zip(self.lower, self.upper):
            if l >= u:
                return True
        return False

    def equals(self, other, tolerance=0):
        for a, b in zip(self.lower, other.lower):
            if abs(a - b) > tolerance:
                return False
        for a, b in zip(self.upper, other.upper):
            if abs(a - b) > tolerance:
                return False
        return True

# The NxLib transformation in the JSON file is in millimeters. We # represent it in meters like everything in ROS. Get the inverse of the pose. Check whether this pose is equal to another one. The poses are         considered equal, when all of the entries are within the given tolerance         of each other. Check whether the points are the same, up to the given distance.