import unittest
                
from LinearAlgebra.Quaternion import *

class QuaternionTest(unittest.TestCase):
    def test_Equality(self):
        q1 = Quaternion.identity
        q2 = Quaternion(1, 0, 0, 0)

        assert(q1 != q2)

        q2 = Quaternion(0, 0, 0, 1)
        assert(q1 == q2)

    def test_FromAngles(self):
        q = Quaternion.FromAngles(Angle.zero, Angle.zero, Angle.zero)
        assert(q == Quaternion.identity)

        q = Quaternion.FromAngles(Angle.Degrees(90), Angle.Degrees(90), Angle.Degrees(-90))
        sqrt2_2 = math.sqrt(2) / 2
        assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
        
    def test_ToAngles(self):
        q1 = Quaternion.identity
        angles = Quaternion.ToAngles(q1)
        assert(angles == (Angle.zero, Angle.zero, Angle.zero))

        q1 = Quaternion(1, 0, 0, 0)
        angles = Quaternion.ToAngles(q1)
        assert(angles == (Angle.zero, Angle.Degrees(180), Angle.zero))

    def test_Degrees(self):
        q = Quaternion.Degrees(0, 0, 0)
        assert(q == Quaternion.identity)

        q = Quaternion.Degrees(90, 90, -90)
        sqrt2_2 = math.sqrt(2) / 2
        assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
        # assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))

    def test_Radians(self):
        q = Quaternion.Radians(0, 0, 0)
        assert(q == Quaternion.identity)

        q = Quaternion.Radians(math.pi / 2, math.pi / 2, -math.pi / 2)
        sqrt2_2 = math.sqrt(2) / 2
        assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
        # assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))

    def test_Multiply(self):
        q1 = Quaternion.identity
        q2 = Quaternion(1, 0, 0, 0)

        r = q1 * q2;
        assert(r == Quaternion(1, 0, 0, 0))
   
    def test_MultiplyVector(self):
        v1 = Vector3.up

        q1 = Quaternion.identity
        v = q1 * v1
        assert(v == Vector3(0, 1, 0))

        q1 = Quaternion(1, 0, 0, 0)
        v = q1 * v1
        assert(v == Vector3(0, -1, 0))

    def test_Angle(self):
        q1 = Quaternion.identity
        q2 = Quaternion.identity
        r = Quaternion.Angle(q1, q2)
        assert(r == Angle.zero)

    def test_AngleAround(self):
        axis = Direction.up
        q1 = Quaternion.identity

        angle = q1.AngleAround(axis)
        assert(angle == Angle.Degrees(0))

        sqrt2_2 = math.sqrt(2) / 2
        q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
        angle = q1.AngleAround(axis)
        assert(angle == Angle.Degrees(180))

        axis = Direction.zero
        angle = q1.AngleAround(axis)
        assert(math.isnan(angle.InDegrees()))

    def test_RotationAround(self):
        axis = Direction.up
        q1 = Quaternion.identity

        q = q1.RotationAround(axis)
        assert(q == Quaternion.identity)

        sqrt2_2 = math.sqrt(2) / 2
        q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0)
        q = q1.RotationAround(axis)
        assert(q == Quaternion(0, 1, 0, 0))

        axis = Direction.zero
        q = q1.RotationAround(axis);
        assert(math.isnan(q.x) and math.isnan(q.y) and math.isnan(q.z) and math.isnan(q.w))