# Standard Library
from math import pi as M_PI
from math import cos, sin

class Quaternion(object):
    @classmethod
    def from_rpy(cls, r, p, y):
        cr = cos(r * 0.5)
        sr = sin(r * 0.5)
        cp = cos(p * 0.5)
        sp = sin(p * 0.5)
        cy = cos(y * 0.5)
        sy = sin(y * 0.5)

        return cls(
            w=cy * cr * cp + sy * sr * sp,
            x=cy * sr * cp - sy * cr * sp,
            y=cy * cr * sp + sy * sr * cp,
            z=sy * cr * cp - cy * sr * sp)

    @classmethod
    def _conjugate(cls, q):
        return cls(q.w, -q.x, -q.y, -q.z)

    @classmethod
    def _mul(cls, q0, q1):
        return cls(
            w=q0.w*q1.w - q0.x*q1.x  - q0.y*q1.y  - q0.z*q1.z
            x=q0.w*q1.x + q0.x*q1.w  - q0.y*q1.z  + q0.z*q1.y
            y=q0.w*q1.y + q0.x*q1.z  + q0.y*q1.w  - q0.z*q1.x
            z=q0.w*q1.z - q0.x*q1.y  + q0.y*q1.x  + q0.z*q1.w)

    @classmethod
    def _rot(cls, q, v):
        q2 = cls(w=0, x=v[0], y=v[1], z=v[2])
        # q * v * q^-1
        return cls._mul(cls._mul(q, q2), cls_.conjugate(q))[1:]

    def __init__(self, w, x, y, z):
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def __iter__(self):
        return (self.w, self.x, self.y, self.z)

    def __mul__(self, other):
        if isinstance(other, type(self)):
            return __class__._mul(self, other)
        elif isinstance(other, tuple):
            return __class__._rot(self, other)

    def inv(self):
        return __class__._conjugate(self)