def test_quaternion_rot_matrix_det_one(self):
   ''' Test that the determinant of the rotation matrix is 1.'''
   for _ in range(10):
     theta = np.random.normal(0., 1., 4)
     theta = Quaternion(theta/np.linalg.norm(theta))
     R = theta.rotation_matrix()
     self.assertAlmostEqual(np.linalg.det(R), 1.0)

 def __init__(self, src=None, timestamp=None):
     if src is not None:
         Quaternion.__init__(self, src.w, src.x, src.y, src.z)
     else:
         Quaternion.__init__(self)
     self.timestamp = timestamp
     self.valid = True
     self.interpolated = False

def generate_quaternion():
	q1 = Quaternion.random()
	q2 = Quaternion.random()
	while True:
		for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True):
			yield q
		#q1, q2 = q2, q1
		q1 = q2
		q2 = Quaternion.random()

 def test_q_bar_matrix(self):
     a, b, c, d = randomElements()
     q = Quaternion(a, b, c, d)
     M = np.array([   
         [a, -b, -c, -d],
         [b,  a,  d, -c],
         [c, -d,  a,  b],
         [d,  c, -b,  a]])
     self.assertTrue(np.array_equal(q._q_bar_matrix(), M))

    def test_inverse(self):
        q1 = Quaternion(randomElements())
        q2 = Quaternion.random()
        if q1:
            self.assertEqual(q1 * q1.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
        else:
            with self.assertRaises(ZeroDivisionError):
                q1 * q1.inverse()

        self.assertEqual(q2 * q2.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))

def fqa(msr):
    ax = msr['accel.x']
    ay = msr['accel.y']
    az = msr['accel.z']
    norm = math.sqrt(ax*ax + ay*ay + az*az)
    # estimation of elevation quaternion (around y axes)
    sin_theta_a = (ax / norm)
    cos_theta_a = math.sqrt(1 - sin_theta_a ** 2)
    sin_half_theta = math.copysign(1, sin_theta_a) * math.sqrt((1 - cos_theta_a) / 2)
    cos_half_theta = math.sqrt((1 + cos_theta_a) / 2)
    q_e = Quaternion(cos_half_theta, 0, sin_half_theta, 0)
    # estimation of roll quaternion (around x axis)
    sin_phi = (-ay / norm) / cos_theta_a
    cos_phi = (-az / norm) / cos_theta_a
    sin_half_phi = half_sin(sin_phi, cos_phi)
    cos_half_phi = half_cos(sin_phi, cos_phi)
# TODO singularity avoidance!!!
    q_r = Quaternion(cos_half_phi, sin_half_phi, 0, 0)
    # estimation of azimuth quaternion (around z axis)
    mx = msr['mag.x']
    my = msr['mag.y']
    mz = msr['mag.z']
    norm = math.sqrt(mx*mx + my*my + mz*mz)
    qm = Quaternion(0, mx / norm, my / norm, mz / norm)
    qm_a = q_e.multiply(q_r).multiply(qm).multiply(q_r.inverse()).multiply(q_e.inverse())
    Quaternion.normalize(qm_a)
    cos_zeta = qm_a.c
    sin_zeta = qm_a.b
    sin_half_zeta = half_sin(sin_zeta, cos_zeta)
    cos_half_zeta = half_cos(sin_zeta, cos_zeta)
    q_a = Quaternion(cos_half_zeta, 0, 0, sin_half_zeta)
    q_fqa = q_a.multiply(q_e).multiply(q_r)
    return q_fqa

 def update(self, msr):
     t = msr['t']
     dt = t - self.t
     self.t = t
     w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
     q_fqa = fqa(msr)
     dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
     dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
     dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
     self.q = self.q.add(dq.scalar_multiply(dt))
     self.q = Quaternion.normalize(self.q)

    def test_matrix_io(self):
        v = np.random.uniform(-100, 100, 3)

        for i in range(10):
            q0 = Quaternion.random()
            R  = q0.rotation_matrix()
            q1 = Quaternion(matrix=R)
            np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)

            self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations

 def __init__(self, cx, cy, radius):
     """
     Initialize instance of ArcBall with no constraint on axis of rotation
     """
     self.center_x = cx
     self.center_y = cy
     self.radius = radius
     self.v_down = PVector()
     self.v_drag = PVector()
     self.q_now = Quaternion()
     self.q_down = Quaternion()
     self.q_drag = Quaternion() 
     self.axis_set = [PVector(1.0, 0.0, 0.0), PVector(0.0, 1.0, 0.0), PVector(0.0, 0.0, 1.0)]
     self.axis = -1  

    def get(self, requested_timestamp):
        if len(self.history) == 0:
            return None

        if requested_timestamp in self.history:
            match = self.history[requested_timestamp]
            return TimestampedQuaternion(match.w, match.x, match.y, match.z, requested_timestamp)
        else:

            keys = list(self.history.keys())
            keys.sort()

            if keys[0] > requested_timestamp or keys[-1] < requested_timestamp:
                return None

            for i, key in enumerate(keys):
                if key > requested_timestamp:
                    previous_timestamp = keys[i - 1]
                    next_timestamp = key
                    timestamp_delta = next_timestamp - previous_timestamp
                    requested_timestamp_offset = requested_timestamp - previous_timestamp
                    requested_timestamp_ratio = requested_timestamp_offset / timestamp_delta
                    interpolated_quaternion = Quaternion.slerp(self.history[previous_timestamp],
                                                               self.history[next_timestamp],
                                                               requested_timestamp_ratio)

                    match = TimestampedQuaternion(interpolated_quaternion.w,
                                                  interpolated_quaternion.x,
                                                  interpolated_quaternion.y,
                                                  interpolated_quaternion.z, requested_timestamp)
                    match.setInterpolated(True)
                    return match
            return None

    def test_differentiation(self):
        q = Quaternion.random()
        omega = np.random.uniform(-1, 1, 3) # Random angular velocity

        q_dash = 0.5 * q * Quaternion(vector=omega)

        self.assertEqual(q_dash, q.derivative(omega))

  def test_quaternion_inverse(self):
    '''Test that the quaternion inverse works.'''
    # First construct any random unit quaternion. Not uniform.
    s = 2*random.random() - 1.
    p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
    p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() - 
          (1. - np.abs(s) - np.abs(p1)))
    p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
    theta = Quaternion(np.array([s, p1, p2, p3]))

    theta_inv = theta.inverse()
    
    identity = theta*theta_inv
    self.assertAlmostEqual(identity.s, 1.0)
    self.assertAlmostEqual(identity.p[0], 0.0)
    self.assertAlmostEqual(identity.p[1], 0.0)
    self.assertAlmostEqual(identity.p[2], 0.0)

  def test_quaternion_rotation_angle(self):
    ''' Test generating rotation angle from quaternion. '''
    # First construct any random unit quaternion. Not uniform.
    s = 2*random.random() - 1.
    p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
    p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() - 
          (1. - np.abs(s) - np.abs(p1)))
    p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
    theta = Quaternion(np.array([s, p1, p2, p3]))

    rotation_angle = theta.rotation_angle()
    phi = Quaternion.from_rotation(rotation_angle)

    self.assertAlmostEqual(phi.s, theta.s)
    self.assertAlmostEqual(phi.p[0], theta.p[0])
    self.assertAlmostEqual(phi.p[1], theta.p[1])
    self.assertAlmostEqual(phi.p[2], theta.p[2])

class ComplimentaryFilter:
    def __init__(self, k=0.5):
        self.q = Quaternion(1, 0, 0, 0)
        self.t = 0
        self.k = k

    def update(self, msr):
        t = msr['t']
        dt = t - self.t
        self.t = t
        w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
        q_fqa = fqa(msr)
        dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
        dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
        dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
        self.q = self.q.add(dq.scalar_multiply(dt))
        self.q = Quaternion.normalize(self.q)

    def test_interpolate(self):
        q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
        q2 = Quaternion(axis=[1, 0, 0], angle=2*pi/3)
        num_intermediates = 3
        base = pi/6
        list1 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=False))
        list2 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=True))
        self.assertEqual(len(list1), num_intermediates)
        self.assertEqual(len(list2), num_intermediates+2)
        self.assertEqual(list1[0], list2[1])
        self.assertEqual(list1[1], list2[2])
        self.assertEqual(list1[2], list2[3])

        self.assertEqual(list2[0], q1)
        self.assertEqual(list2[1], Quaternion(axis=[1, 0, 0], angle=base))
        self.assertEqual(list2[2], Quaternion(axis=[1, 0, 0], angle=2*base))
        self.assertEqual(list2[3], Quaternion(axis=[1, 0, 0], angle=3*base))
        self.assertEqual(list2[4], q2)

 def test_init_copy(self):
     q1 = Quaternion.random()
     q2 = Quaternion(q1)
     self.assertIsInstance(q2, Quaternion)
     self.assertEqual(q2, q1)
     with self.assertRaises(TypeError):
         q3 = Quaternion(None)
     with self.assertRaises(ValueError):
         q4 = Quaternion("String")

 def estimate_divergence(self):
   ''' 
   Estimate the divergence term with a deterministic
   finite difference approach. This is for a single quaternion.
   '''
   delta = 1e-6
   div_term = np.zeros(3)
   for k in range(3):
     omega = np.zeros(3)
     omega[k] = 1.
     quaternion_dt = Quaternion.from_rotation(omega*delta/2.)
     quaternion_tilde_1 = quaternion_dt*self.orientation[0]
     quaternion_dt = Quaternion.from_rotation(-1.*omega*delta/2.)
     quaternion_tilde_2 = quaternion_dt*self.orientation[0]
     div_term += np.inner((self.mobility([quaternion_tilde_1]) -
                           self.mobility([quaternion_tilde_2])),
                          omega/delta)
     
   return div_term

    def test_conjugate(self):
        a, b, c, d = randomElements()
        q1 = Quaternion(a, b, c, d)
        q2 = Quaternion.random()
        self.assertEqual(q1.conjugate(), Quaternion(a, -b, -c, -d))

        self.assertEqual((q1 * q2).conjugate(), q2.conjugate() * q1.conjugate())
        self.assertEqual((q1 + q1.conjugate()) / 2, Quaternion(scalar=q1.scalar()))
        self.assertEqual((q1 - q1.conjugate()) / 2, Quaternion(vector=q1.vector()))

 def test_quaternion_from_rotation(self):
   ''' Test that we correctly create a quaternion from an angle '''
   # Generate a random rotation vector
   phi = np.random.rand(3)
   phi_norm = np.linalg.norm(phi)
   theta = Quaternion.from_rotation(phi)
   
   self.assertAlmostEqual(theta.entries[0], np.cos(phi_norm/2.))
   self.assertAlmostEqual(theta.entries[1], np.sin(phi_norm/2.)*phi[0]/phi_norm)
   self.assertAlmostEqual(theta.entries[2], np.sin(phi_norm/2.)*phi[1]/phi_norm)
   self.assertAlmostEqual(theta.entries[3], np.sin(phi_norm/2.)*phi[2]/phi_norm)

  def test_quaternion_rotation_matrix(self):
    ''' Test that we create the correct rotation matrix for a quaternion. '''
    # First construct any random unit quaternion. Not uniform.
    s = 2*random.random() - 1.
    p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
    p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() - 
          (1. - np.abs(s) - np.abs(p1)))
    p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
    theta = Quaternion(np.array([s, p1, p2, p3]))
    
    R = theta.rotation_matrix()

    self.assertAlmostEqual(R[0][0], 2.*(theta.s**2 + theta.p[0]**2 - 0.5))
    self.assertAlmostEqual(R[0][1], 2.*(theta.p[0]*theta.p[1] - 
                                        theta.s*theta.p[2]))
    self.assertAlmostEqual(R[1][0], 2.*(theta.p[0]*theta.p[1] +
                                        theta.s*theta.p[2]))
    self.assertAlmostEqual(R[1][1], 2.*(theta.s**2 + theta.p[1]**2 - 0.5))
    self.assertAlmostEqual(R[2][2], 2.*(theta.s**2 + theta.p[2]**2 - 0.5))
    self.assertAlmostEqual(R[2][0], 2.*(theta.p[0]*theta.p[2] - theta.s*theta.p[1]))