import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
                
from SwingTwist import *

class SwingTwistTest(unittest.TestCase):
    def test_Constructor(self):
        s = SwingTwist.Degrees(0, 0, 0)
        assert(s == SwingTwist.Degrees(0, 0, 0))

        s = SwingTwist.Degrees(0, 180, 0)
        assert(s == SwingTwist.Degrees(180, 0, 180))

        s = SwingTwist.Degrees(0, 180, 180)
        assert(s == SwingTwist.Degrees(180, 0, 0))

        s = SwingTwist.Degrees(270, 90, 0)
        assert(s == SwingTwist.Degrees(-90, 90, 0))

        s = SwingTwist.Degrees(270, 270, 0)
        assert(s == SwingTwist.Degrees(-90, -90, 0))

        s = SwingTwist.Degrees(270, 225, 0)
        assert(s == SwingTwist.Degrees(90, -45, -180))

        s = SwingTwist.Degrees(270, 0, 225)
        assert(s == SwingTwist.Degrees(-90, 0, -135))

    def test_FromQuaternion(self):
        q = Quaternion.identity
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(0, 0, 0))

        q = Quaternion.Degrees(90, 0, 0)
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(90, 0, 0))

        q = Quaternion.Degrees(0, 90, 0)
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(0, 90, 0))

        q = Quaternion.Degrees(0, 0, 90)
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(0, 0, 90))

        q = Quaternion.Degrees(0, 180, 0)
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(0, 180, 0))

        q = Quaternion.Degrees(0, 135, 0)
        r = SwingTwist.FromQuaternion(q)
        assert(r == SwingTwist.Degrees(0, 135, 0))

    def test_FromAngleAxis(self):
        r = SwingTwist.FromAngleAxis(Angle.zero, Direction.up)
        assert(r == SwingTwist.Degrees(0, 0, 0))

        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.up)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(90, 0, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.up)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(180, 0, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.up);
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(-90, 0, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.right)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 90, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.right)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 180, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.right)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, -90, 0))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.forward)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 90))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.forward)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 180))
        assert(angle.InDegrees() == 0)

        r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.forward)
        angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, -90))
        assert(angle.InDegrees() == 0)

        # auto r16 = SwingTwist16::AngleAxis(13, Direction16::down);
        # auto s16 = SwingTwist16::Degrees(-13, 0, 0);
        # assert(SwingTwist16::Angle(r16, s16), Angle16::Degrees(0))