from LinearAlgebra.Direction import Direction
from LinearAlgebra.Quaternion import Quaternion

class SwingTwist:
    """A rotation using swing and twist angle components"""
    def __init__(self, swing: Direction, twist: float):
        if swing.vertical > 90 or swing.vertical < -90:
            swing.horizontal += 180
            swing.vertical = 180 - swing.vertical
            twist += 180        

        ## Swing component of the rotation
        self.swing = swing
        ## The twist component of the rotation
        self.twist = twist
    
    @staticmethod
    def Degrees(horizontal: float, vertical: float, twist: float):
        direction = Direction(horizontal, vertical)
        swing_twist = SwingTwist(direction, twist)
        return swing_twist

    def ToQuaternion(self) -> Quaternion:
        """Convert the SwingTwist rotation to a Quaternion"""
        q = Quaternion.Euler(-self.swing.vertical,
                             self.swing.horizontal,
                             self.twist)
        return q