from .Direction import *
from .Quaternion import *

class SwingTwist:
    """A rotation using swing and twist angle components"""
    def __init__(self, swing: Direction, twist: Angle):
        ## 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):
        horizontal_angle = Angle.Degrees(horizontal)
        vertical_angle = Angle.Degrees(vertical)
        twist_angle = Angle.Degrees(twist)

        deg90 = Angle.Degrees(90)
        deg180 = Angle.Degrees(180)
        if vertical_angle > deg90 or vertical_angle < -deg90:
            horizontal_angle += deg180
            vertical_angle = deg180 - vertical_angle
            twist_angle += deg180

        direction = Direction(horizontal_angle, vertical_angle)
        swing_twist = SwingTwist(direction, twist_angle)
        return swing_twist
    @staticmethod
    def Radians(horizontal: float, vertical: float, twist: float):
        horizontal_angle = Angle.Radians(horizontal)
        vertical_angle = Angle.Radians(vertical)
        twist_angle = Angle.Radians(twist)

        deg90 = Angle.Radians(math.pi / 2)
        deg180 = Angle.Radians(math.pi)
        if vertical_angle > deg90 or vertical_angle < -deg90:
            horizontal_angle += deg180
            vertical_angle = deg180 - vertical_angle
            twist_angle += deg180

        direction = Direction(horizontal_angle, vertical_angle)
        swing_twist = SwingTwist(direction, twist_angle)
        return swing_twist

    def ToQuaternion(self) -> Quaternion:
        """Convert the SwingTwist rotation to a Quaternion"""
        q = Quaternion.FromAngles(
            -self.swing.vertical,
            self.swing.horizontal,
            self.twist
        )
        return q
    @staticmethod
    def FromQuaternion(q: Quaternion):
        angles = Quaternion.ToAngles(q)
        # direction = Direction(angles[0], angles[1])
        # r: SwingTwist = SwingTwist(direction, angles[2])
        r = SwingTwist.Degrees(
            angles[0].InDegrees(),
            angles[1].InDegrees(),
            angles[2].InDegrees()
        )
        return r
 
    def FromAngleAxis(angle: Angle, axis: Direction):
        vectorAxis: Vector3 = axis.ToVector3();
        q: Quaternion = Quaternion.FromAngleAxis(angle, vectorAxis);
        return SwingTwist.FromQuaternion(q)


    def __eq__(self, other) -> bool:
        """! Check if this orientation is equal to the given orientation
        @param other The orientation to check against
        @return true if it is identical to the given orientation
        @note This uses float comparison to check equality which may have strange
        effects. Equality on floats should be avoided.
        """
        return (
            self.swing == other.swing and
            self.twist == other.twist
        )    
    
    @staticmethod
    def Angle(r1, r2) -> Angle:
        q1: Quaternion = r1.ToQuaternion()
        q2: Quaternion = r2.ToQuaternion()
        angle: float = Quaternion.Angle(q1, q2)
        return angle