'''
Classes supporting a geometry in R3 space:
    R3Vector, R3Point
'''
from math import sqrt, atan2

R3_EPSILON = 1.0e-8

class R3Vector:
    '''Vector on R3-Plane'''

    def __init__(self, x = 0., y = 0., z = 0.):
        if hasattr(x, "__getitem__"):
            self.x = float(x[0])
            self.y = float(x[1])
            self.z = float(x[2])
        else:
            self.x = float(x)
            self.y = float(y)
            self.z = float(z)

    def copy(self):
        '''Shallow copy of a vector'''
        return R3Vector(self.x, self.y, self.z)

    def __getitem__(self, idx):
        if idx == 0:
            return self.x
        elif idx == 1:
            return self.y
        elif idx == 2:
            return self.z
        else:
            raise IndexError("Vector index out of range")

    def __setitem__(self, idx, v):
        if idx == 0:
            self.x = float(v)
        elif idx == 1:
            self.y = float(v)
        elif idx == 2:
            self.z = float(v)
        else:
            raise IndexError("Vector index out of range")

    def __add__(self, v):
        assert type(v) == R3Vector or type(v) == R3Point
        if type(v) == R3Vector:
            return R3Vector(self.x + v.x, self.y + v.y, self.z + v.z)
        else:
            return R3Point(self.x + v.x, self.y + v.y,  self.z + v.z)

    def __iadd__(self, v):
        assert type(v) == R3Vector
        self.x += v.x; self.y += v.y; self.z += v.z
        return self

    def __sub__(self, v):
        assert type(v) == R3Vector
        return R3Vector(self.x - v.x, self.y - v.y, self.z - v.z)

    def __isub__(self, v):
        assert type(v) == R3Vector
        self.x -= v.x; self.y -= v.y; self.z -= v.z
        return self

    def __neg__(self):
        return R3Vector(-self.x, -self.y, -self.z)

    def __mul__(self, v):
        if type(v) == R3Vector:
            '''Dot (scalar) product of 2 vectors'''
            return self.x * v.x + self.y * v.y + self.z * v.z
        else:
            '''Multiply a vector by a number'''
            return R3Vector(self.x*float(v), self.y*float(v), self.z*float(v))

    def __rmul__(self, v):
        if type(v) == R3Vector:
            '''Dot (scalar) product of 2 vectors'''
            return v.x*self.x + v.y*self.y + v.z*self.z
        else:
            '''Multiply a vector by a number'''
            return R3Vector(float(v)*self.x, float(v)*self.y,  float(v)*self.z)

    def __imul__(self, a):
        '''Multiply a vector by a number'''
        self.x *= float(a)
        self.y *= float(a)
        self.z *= float(a)
        return self

    def dot(self, v):
        '''Dot (scalar) product of 2 vectors'''
        return self.x * v[0] + self.y * v[1] + self.z * v[2]

    def __matmul__(self, v):
        return self.dot(v)

    def __str__(self):
        return (
            "(" + str(self.x) + ", " + 
            str(self.y) + ", " +
            str(self.z) + ")"
        )

    def __repr__(self):
        return "R3Vector" + str(self)

    def length(self):
        return sqrt(self.x*self.x + self.y*self.y + self.z*self.z)

    def norm(self):
        return self.length()

    def normalize(self):
        ''' Normalization of vector: make its length == 1,
        preserving its direction'''
        l = self.length()
        if l > 0.:
            self.x /= l
            self.y /= l
            self.z /= l
        return self

    def normalized(self):
        '''Return a unit vector with the same direction'''
        l = self.length()
        if l > 0.:
            l = 1./l
        else:
            l = 1.
        return self*l

    def cross(self, v):
        '''Return a cross product of 2 vectors'''
        return R3Vector(
            self.y*v[2] - self.z*v[1],
            -self.x*v[2] + self.z*v[0],
            self.x*v[1] - self.y*v[0]
        )
    def vectorProduct(self, v):
        return self.cross(v)

    def angle(self, v):
        '''Angle between two vectors in radians'''
        assert type(self) == R3Vector and type(v) == R3Vector
        len0 = self.length()
        len1 = v.length()
        if len0 <= R3_EPSILON or len1 <= R3_EPSILON:
            return 0;
        ex = self * (1./len0);
        w = v * (1./len1);
        ez = ex.cross(w);
        lenz = ez.length();
        if lenz <= R3_EPSILON:
            return 0.
        ez *= (1./lenz);
        ey = ez.cross(ex);
        return abs(atan2(
            w*ey, w*ex
        ))

    def area(self, v):
        w = self.cross(v)
        return w.length()

    def __gt__(self, v):
        return (self.x, self.y, self.z) > (v[0], v[1], v[2])

    def __ge__(self, v):
        return (self.x, self.y, self.z) >= (v[0], v[1], v[2])

    def __lt__(self, v):
        return not (self >= v)

    def __le__(self, v):
        return not (self > v)

    def __eq__(self, v):
        return (not (self < v)) and (not (v < self))

    def __ne__(self, v):
        return not (self == v)

class R3Point:
    '''Point in R3-Space'''

    def __init__(self, x = 0., y = 0., z = 0.):
        if hasattr(x, "__getitem__"):
            self.x = float(x[0])
            self.y = float(x[1])
            self.z = float(x[2])
        else:
            self.x = float(x)
            self.y = float(y)
            self.z = float(z)

    def copy(self):
        '''Shallow copy of a point'''
        return R3Point(self.x, self.y, self.z)

    def __getitem__(self, idx):
        if idx == 0:
            return self.x
        elif idx == 1:
            return self.y
        elif idx == 2:
            return self.z
        else:
            raise IndexError("Vector index out of range")

    def __setitem__(self, idx, v):
        if idx == 0:
            self.x = float(v)
        elif idx == 1:
            self.y = float(v)
        elif idx == 2:
            self.z = float(v)
        else:
            raise IndexError("Vector index out of range")

    def __add__(self, v):
        return R3Point(self.x + v[0], self.y + v[1], self.z + v[2])

    def __radd__(self, v):
        return R3Point(v[0] + self.x, v[1] + self.y, v[2] + self.z)

    def __iadd__(self, v):
        self.x += v[0]; self.y += v[1]; self.z += v[2]
        return self

    def __sub__(self, v):
        if type(v) == R3Vector:
            return R3Point(self.x - v[0], self.y - v[1], self.z - v[2])
        else:
            # assert type(v) == R3Point
            return R3Vector(self.x - v[0], self.y - v[1], self.z - v[2])

    def __rsub__(self, v):
        # assert type(v) == R3Point
        return R3Vector(v[0] - self.x, v[1] - self.y, v[2] - self.z)

    def __neg__(self):
        return R3Point(-self.x, -self.y, -self.z)

    def __isub__(self, v):
        # assert type(v) == R3Vector
        self.x -= v.x; self.y -= v.y; self.z -= v.z
        return self

    def __mul__(self, a):
        '''Multiply the point coordinates by a number'''
        return R3Point(self.x*float(a), self.y*float(a), self.z*float(a))

    def __rmul__(self, a):
        '''Multiply the point coordinates by a number'''
        return R3Point(float(a)*self.x, float(a)*self.y, float(a)*self.z)

    def __imul__(self, a):
        '''Multiply a point coordinates by a number'''
        self.x *= float(a)
        self.y *= float(a)
        self.z *= float(a)
        return self

    def __str__(self):
        return (
            "(" + str(self.x) + ", " + 
            str(self.y) + ", " +
            str(self.z) + ")"
        )

    def __repr__(self):
        return "R3Point" + str(self)

    def distance(self, p):
        return (p - self).norm()

    def distanceToLine(self, p, v):
        '''Distance from this point to a line defined by
           a point p and a vector v'''
        u = p - self
        if u.norm() <= R3_EPSILON:
            return 0.
        ex = v.normalized()
        ez = u.cross(v)
        if ez.norm() <= R3_EPSILON:
            # The point self belongs to the line (p, v)
            return 0.
        ez.normalize()
        ey = ez.cross(ex) # Normal to the line in the plane
        return abs(u.dot(ey))

    def distanceToPlane(self, q, n):
        '''Return the distance from the point self to the plane
        defined by the point q and the normal vector n'''
        nn = n.normalized()
        return abs((q - self).dot(nn))

    def __gt__(self, p):
        return (self.x, self.y, self.z) > (p[0], p[1], p[2])

    def __ge__(self, p):
        return (self.x, self.y, self.z) >= (p[0], p[1], p[2])

    def __lt__(self, p):
        return not (self >= p)

    def __le__(self, p):
        return not (self > p)

    def __eq__(self, v):
        return (not (self < v)) and (not (v < self))

    def __ne__(self, v):
        return not (self == v)

    @staticmethod
    def area(p1, p2, p3):
        return ((p2 - p1).area(p3 - p1))/2.0

def intersectLineAndPlane(p, v, q, n):
    '''Intersect line and plane
    Line is defined by a point p and a direction vector v
    Plane is defined by a point q and a normal vector n.
    Return the tuple (res, h), where res is boolean value:
    if the line is not parallel to plane,
    then res = True and h is the intersection point;
    otherwise res = False and h = None'''

    s = v.dot(n)
    if abs(s) <= R3_EPSILON:
        return (False, None)

    # h = p + v*t
    # (h - q)*n = 0
    # (p + v*t - q)*n = 0
    # (v*n)*t + (p - q)*n = 0
    # t = ((q - p)*n)/(v*n)

    t = (q - p).dot(n)/s
    h = p + v*t
    return (True, h)

def intersectPlanes(p0, n0, p1, n1):
    '''Intersect planes
    First plane is defined by the point p0
    and the normal vector n0, second plane by
    the point p1 and normal vector n1.
    Return the tuple (res, p, v), where
        bool res = True, when the planes are not parallel,
                         otherwise res = False.
        The pair (p, v) defines the intersection line:
            p is the point on the line, and
            v is the direction vector.'''

    if n0.area(n1) <= R3_EPSILON:
        return (False, None, None)
    v = n0.cross(n1).normalize();
    v1 = n0.cross(v);

    # p = p0 + v1*t
    # ((p0 + v1*t) - p1, n1) = 0
    # (p0 - p1, n1) + (v1, n1)*t = 0
    # t = (p1 - p0, n1) / (v1, n1)
    s = v1.dot(n1)
    if abs(s) <= R3_EPSILON:
        return (False, None, None)

    t = (p1 - p0).dot(n1)/s
    p = p0 + v1*t
    return (True, p, v)